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The feasibility of controlling a robot manipulator in minimum time with an 
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I. INTRODUCTION 



In motion execution of a robot arm, one basic problem is to design a control 
system. This problem is complicated because the dynamic motion of a manipulator is 
strongly influenced by mechanical design and physical properties of the manipulator, as 
well as environmental effects. Coupling inertia, joint friction, coriolis forces, centripetal 
forces, actuator dynamics and gravity effects are considered among these factors. 
These factors produce an interactive nonlinear dynamic system. Under these 
circumstances a robust and flexible control system is needed. 

The curve following scheme is used in all high performance disk files as part of 
the read; write head positioning servo. It has been shown [Ref 1] that the entire loop 
can be modelled in a computer or microprocessor and the model can be used to drive a 
motor. Algorithms were developed to adapt the computer model for small parameter 
discrepancies and to update the states in the computer. In the robot arm there will be 
large parameter variations in mass and moment of inertia due to loading and the 
nonlinear dynamics of the manipulator. 

The purpose of this thesis is to investigate the possibility of using the Curve 
Following Velocity Loop Servo to obtain near minimum time positioning of a robot 
manipulator. In this study, a two link robot manipulator with two degrees of freedom 
is chosen as the simulation model. The same control scheme is used to drive both 
motors. 

Figure 1.1 is a simplified block diagram of the control scheme to be investigated. 
The second order model is a simple approximation of the actuator. A Velocity 
feedback loop is used in the adaptive model only. The "Adaptive Algorithm" uses 
Actuator position output to calculate the Actuator velocity, and updates the second 
order model gain constant, position and velocity at certain time intervals. Therefore 
the requirement for a tachometer is eliminated. The "Curve" approximates the 
deceleration curve of an ideal motor. It is chosen as a parabola. The "Environment" 
represents Gravitational torques. Centripetal forces, Coriolis forces and Inertia loading. 
The "Actuator" includes the servo motor and load. Both the second order model and 
the actuator are driven by the same velocity error input. 
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Figure 1.1 Simplified Block Diagram of the System. 

Development of the computer simulation model is presented in Chapter II. 
Chapter III discusses the modelling of the robot arm. The computer adaptive model is 
developed in Chapter IV. Simulation studies are planned in Chapter V. Simulations 
under different conditions for a Voltage Source Drive System are studied in Chapter 
VI. Chapter VII discusses the development of a Current Source Drive System and 
presents simulation studies under different conditions. Conclusions and further study 
areas are presented in Chapter VIII. DSL simulation programs and derivation of 
mathematical model for robot arm are given in .Appendixes .A — F. 
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II. DEVELOPMENT OF THE COMPUTER SIMULATION MODEL 



A. INTRODUCTION 

It can be shown [Ref. 2] that the equivalent transfer function of the servo motor 
is 



6(s) 

W) 



I'K 



V 



JR 



+ 1 



L 

R 




{eqn 2.1) 



where parameters 0, V, K.^., K^. J, R. and L are defined in Chapter VII. The arm of a 
robot is a large inertia, and when this is added to the motor inertia the mechanical pole 
of the motor becomes smaller. Since R, L is normally a large number, the electrical 
pole of the motor may be neglected and the transfer function of the robot arm and 
motor becomes appro.ximately 



~ -Em 

V(s) S- 



{eqn 2.2) 



Figure 2.1 shows the second order model with a curve following velocity loop. 
The Curve used is the deceleration curve for the idealized motor. Bang-Bang control is 
chosen to drive the system with full power. [Ref 3] 

For a given step position command, the error signal (E) will produce a velocity 

command input. This signal saturates the amplifier and drives the system into full 

# 

acceleration. When the commanded velocity signal (X) becomes equal to the velocity 
feedback signal (KP) the system changes into curve following mode and follows the 
curve until the desired position is reached. 

B. OBTAINING THE CURVE AND MODEL PARAMETERS 

Selection of the curve must be done in such a wav that the svstem will be able to 
follow the curve under different loading conditions. Since a parabolic curve 
approximates the deceleration curve of an ideal motor, it was chosen as the curve to be 
followed. 
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In the robot arm, simulation studies show that the curve must be shaped due to 
the mechanical design of the arm. actuator parameters and gravity effects. Therefore 
the gain constant (Kl) is used to reshape the curve. In our case K1 was set to a value 
of 0.6, It has been shown [Ref 1] that the equation of the curve was derived from the 
idealized motor equations as follows: 

C = Vsat (eqn 2.3) 

C = J C dt = Vsat t + C(0), (C(0)=0) (eqn 2.4) 



C = J C dt = l/2(Kj^ Vsat r) + C(0) 



(eqn 2.5) 



When we solve Equation 2.4 for t and substitude into Equation 2.5. 



C = 



c- 



2 Vsat 



(eqn 2.6) 



For deceleration from initial conditions with the input R = O', 
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C = -E 



(eqn 2.7) 



C = - E (eqn 2.8) 

finally when we substitude Equation 2.7 and 2.8 into Equation 2.6 we come up with: 

• X = V2Kj^Vsat 4 ^ = commanded velocity 

The second order model gain constant (K^^) is determined by the actuator 
parameters and the effective inertia seen by the actuator. The values of were given 
below (to be derived in Chapter IV. 

• Kj^j = 0.17 rad/volt 

• Kj^-) = 4.0 rad volt 

The saturation limits of the amplifiers are determined by available servo motor 
parameters, mechanical design of the arm. working conditions and curve constant (Kl). 
In this study, the same servo motor is used for both actuators, whether or not gravity 
loading exists. The values of Vsat are: 

• Vsatl = 150 volts, Vsat2 = 50 volts (gravity-free environment) 

• Vsatl = 300 volts, Vsat2 = 150 volts (with gravity loading) 

The gain parameter (K2) of the saturation amplifier is chosen to saturate the 
amplifier for small signals so that it acts as a switch to drive the system with full 
forward and reverse drive signals in the curve following mode. Therefore K2 was set to 
the value of 10.000 to guarantee the saturation of the amplifier. 

The gain of the velocity feedback channel (K) has been given [Ref 1] K = Kl. 
In hardware realization of the system the value of K can be adjusted to compare the 
conunanded velocity signal and the velocity feedback signal. For our purpose 
simulation studies show the best curve following is achieved for K = 1. 

C. SIMULATION STUDIES OF THE MODEL 

The model was simulated using DSL VS. The DSL simulation program is listed 
in Appendix A. Vsat and were set to 150 volts, 4.0 rad. volt respectively for these 
studies. Figure 2.2 shows phase plane trajectories (Angular velocity P versus angular 
position P) for a step position conunand of 1 radian. The figure shows that the model 
angular velocity increases with constant acceleration until the curve is reached. Then 
the voltage applied to the model reverses and the model velocity follows the curve until 
the desired angular position is reached. The step response of the model is shown in 
Figure 2.3. 
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Figure 2.2 Phase Plane trajectories of the Model. 
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Figure 2.3 Step Response of the Model. 



III. MODELLING THE ROBOT ARM 



A. INTRODUCTION 

In this chapter, a mathematical model of a two-degrees-of-freedom planar robot 
arm is developed using lagrangian mechanics. The dynamics equations relate torques 
and forces to positions, velocities and accelerations. We need dynamics equations to 
get the equations of motion of the manipulator. We need to know the relationship 
between torque and acceleration at a joint, and the relationship between torque at one 
joint and acceleration at other joints. We also need to know effects of the gravity 
torques and velocity dependent torques. Velocity dependent torques are typically small 
compared to other system torques. They are important when the manipulator is 
moving at high speed. 

B. MODEL DEVELOPMENT 

The two-degrees-of-freedom planar robot arm considered is illustrated in Figure 
3.1. The motion of the robot arm is constrained to the xy plane. Both joints are 
rotary joints. There are two Links, connecting the two joints and a terminal grasping 
device at JOINT2 with or without load. It is assumed that the links are massless and 
two equivalent masses are lumped at the end of LlNKl and LINK2. The lengths of 
LINKl and LINK2 are denoted dj, d-> and two masses are denoted mj, m^ 
respectively. T^ T, represents the external driving torques to the joints. 

C. DERIVATION OF EQUATION OF MOTION 

The lagrangian L is defined as the difference between the kinetic energy K and 
the potential energy V of the system. L = K — V 

Equation 3.1 defines the Lagrange's Equation 




(eqn 3.1) 



where 



q^ = generalized coordinates 

qj = corresponding velocity 

F: = the corresponding force or torque 
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Figure 3.1 Point Mass Reoresentation of a Two-Degrees-Of-Freedom 

planar Robot Arm. 

L = Langrangian of the system 
n = number of degrees of freedom 

To apply equation 3.1 to the robot arm, the rotation angles 0^ and 0., are chosen 
as the generalized coordinates. The generalized forces F^, F, would be the total 
torques about the axis of rotation of 0j and 0, The detailed derivation is presented in 
.Appendix B. The following pair of second-order nonlinear differential equations has 
been derived and will be used as the basic mathematical model for this thesis. 



(Djj+Jmi)^! “ T| (^12®2 ^122®2~ 

+ 01120109 + Di2i0i09 *^Gi) 

( D22 -^ ni 2^®2 ~ ”^2 ~ ^^ 12^1 ^ 211 ®!^ ^ 2 ^ 



(eqn 3.2) 



(eqn 3.3) 
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where 



7 7 

Djj = ( mj + m2 )d| + m2d2'‘ + 2m2d2d',cos02 

Di 2 = ni2d2^ + m')d 2 d 2 Cos 62 
D 22 = ni2d2“ 

02^)2 ~ rn-)d2d2sin6') 

D2 22~ ~ ni2d 2 d')Sin02 ~ ^121 
D 2 2 [ ~ ^ 2 *^ 1 

G 2 = [(m 2 + rri2)d2cos02 + m2d2cos(02 + 02)]g 

G 2 = ni2d2cos(02 + 02)g 

Jmi = motor inertia for Joint 1 motor 

Jm2 ~ motor inertia for Joint 2 motor 

m 2 = 0.24S oz in sec" 

m:i = 0.041 oz in;sec" 

m 2 = 0.082 oz, in, sec" (with load) 

d 2 =15 inches 

d-) =10 inches 

g = 386.4 in'sec" 

Let us rewrite Equations 3.2 and 3.3 in the form 



Ti = (D 22 + Jpi)02 + Di202 

+ D22O0]0") ^121®1®2 



(eqn 3.4) 



(^22 + -^m2)®2 ^12®! 



+ D 22 J0 
4-G 



(eqn 3.5) 



•In these equations (^ 22 +^ 01 !^ known as the effective 

inertia at joints 1 and 2. Accelerations at these joints cause torques and are referred to 

as inertial torques. D 22 is known as the coupling enertia between JOINTl and 

* • 

JOINT2. D2202 is the reaction torque acting on JOINTl induced by the angular 

• • 

acceleration of JOINT2; C>2202 is the reaction torque acting on JOINT2 induced by 
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* 9 

angular acceleration of JOINTl. Di22®2"^ centripetal force acting on JOINTl 

generated by the angular velocity of JOINT2; D211®1 centripetal force acting 

• • • • 

on JOINT2 due to angular velocity at JOIXTl. D]i2®i®2 ^121^1^2 coriolis 

force acting on JOINTl generated by the angular velocities of both JOINTl and 
JOINT2. Gj and G 2 represent the gravity forces on JOINTl and JOINT2 
respectively. [Ref 4] 
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IV. THE COMPUTER ADAPTIVE MODEL 



A. INTRODUCTION 

The second order model of Chapter II is to be used to drive, open loop, the servo 
motor of one joint of the robot arm. The servo motor has to follow a predetermined 
curve until a desired position is reached. To accomplish this, the second order model 
states and gain constant have to be adapted in such a way, that the model mimics the 
position and velocity and has a gain constant equivalent to that of the actual servo 
motor. In this chapter we will first discuss the selection of servo motors for testing of 
the adaptive systems and will then obtain the algorithm to update the second order 
model position, velocity and gain constant. The calculation of the initial value of the 
model gain constant is also given in this Chapter. 

B. SELECTION OF THE SERVO MOTORS 

A permanent magnet motor drive currently used in industrial robots was selected 
to test the adaptive system. Parametric data for the motor are listed in Table L[Ref 5] 



TABLE 1 

PAR.AMETR1C DATA FOR JOINT SERVO MOTORS 



Torque Constant K.(. 




14. 4 


oz-in/amp 


Total Inertia Jj^ 




0. 033 


oz-in-secV^ad 


Damping Coefficient 


Bm 


0. 04297 


oz- in- sec/rad 


Back emf Const. K.^. 




0. 1012 


volts-sec/rad 


Armature Inductance 


L 


100 


}i-henries 


Ave. Terminal Resistance R 


0. 91 


ohms 



The same motor drive is used for both JOINTl and JOINT2 with a diflerent 
power supply. To calculate the transfer functions of the servo motors, we need to 
know the effective joint inertias: 

• JeFFI = ^11 -^ml 



• -^EFF2 " ^22 ^ ^m2 

Where D||, Dj 2 ^ Jml’ '^m'> given in Chapter II. When we substitute parameters, 
we come up with: 

• JefFI “ 81-425 oz.in.s" (for 02 = 0®) 

• "^EFF^ ~ 4.14 oz.in.s^ 

Substituting effective inertias calculated above and parameters given in Table 1 into 
Equation 2.1, gives us the transfer functions of the servo motors. 



9.S8 

Gl(s) = radians/ volts 

S(S/9100+1)(S;0.019+1) 



(eqn 4. 1) 



9.8S 

G2(s) = radians volts 

S(S;9100+ 1)(S,0.39+ 1) 



(eqn 4.2) 



The Open Loop Bode plots for the servo motors are shown in Figures 4.1 and 
4.2. From the Open Loop Bode plots it can be observed that the - 40dB/decade slope 
crosses the 0.0 dB axis at (Oj = 0.412 rad/sec and (02=2.0 rad/sec for JOINTl and 
JOINT2 servo motors respectively. When we use the <Oj and CO 2 frequencies as the 
gain crossover frequencies of the second-order ideal motors, we come up with: 

• = 0.17 rad/volt 

• Kj ^2 “ 4.0 rad/volt 

Thus the gain constants and K^^2 second-order models were set 

initially to these calculated values to determine the curv’e derived in Chapter II. Open 
Loop Bode diagrams of the K^^/s" ideal motors are given in Figures 4.3 and 4.4. 

C. OBTAINING THE ADAPTIVE ALGORITHM 

Figure 4.5 shows block, diagram of the adaptive system for a joint servo drive. 
The same adaptive scheme is used to drive both servo motors separately. In this 
diagram Tj_^ refers to the sum of disturbance torques such as Centripetal Torques, 
Centrifugal Torques, Reaction Torques and Gravitational Torques. The saturation 
amplifier output signal is common input to both the second-order model and servo 
motor. Output of the servo motor is measured and fed into the Adaptive .Algorithm 
block. Our next step will be to calculate an estimate of the servo motor gain 
parameter (K^^^ and the velocity of the servo motor. In these calculations, two criteria 
must be taken into consideration; 
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Figure 4.1 Open Loop Bode Plot of the JOINTl Servo .Motor. 
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Figure 4.2 



Open Loop Bode Plot of the JOINT2 Servo Motor. 
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Figure 4.3 



Open Loop Bode Plot of the K^j/s" Motor. 
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Figure 4.4 



Open Loop Bode Plot of the 
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Figure 4.5 Block Diagram of the Adaptive Joint Drive System. 
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V. PLANNING THE SIMULATION STUDIES 



A set of sample motions was selected to serve as a basis for simulation studies of 
the system. The sample motions were designed to remove as many of the 
nonlinearities as possible and to investigate the system behavior for possible 
overloading conditions. The set of sample motions used are depicted in Figures 5.1, 5.2 
and 5.3. 

Simulation studies were done for two different drive systems. These were: 

• Voltage Source Drive 

• Current Source Drive 

Each drive system was tested separately for; 

• Gravity- Free Environment 

• With Gravity Loading 

Position Command inputs to the system were: 

• Step Input 

0 Time Varying Input 

In this thesis, a free work space is assumed for the robot arm. To find the 
minimum time response for a given step position command, a step input was applied to 
the servo motors: 

• At the same time 

• At different times 

All simulation studies are planned for both loaded and unloaded robot arms. 
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•. The calculations must be accurate enough to allow the second-order model 
states to approximate the trajector>' of theTervo motor. 

•. The calculations must be kept simple to update the second-order model states 
and gain constant in minimum time. 

There are several means of doing these calculations. For our purpose, the 
method described in [Ref 1] best satisfies the criterion given above. It can be shown 
[Ref 1] that; when we solve Equation 2.5 for K„ 

III, 



K 



m 



2P 

Vsat t- 



(eqn 4.3) 



For discrete time intervals and letting P = P.M, 



K 



m 



2PM 

vsat(XT)- 



(eqn 4.4) 



Where T is the sampling period and X is the number of sampling intervals. Equation 
4.4 is valid only for constant acceleration of the servo motor. Therefore the value of 
is updated during the full acceleration of the servo motor, then remains unchanged 
for the curve following portion. 

The estimation of the angular velocity of the servo motor has been given [Ref 1] 
PM(X) = 2[PM(X)-PM(X- 1)]/T - PM(X-l) (eqn 4.5) 



PM(N - 1) = [PM(X') - PM(N - 2)]/2T (eqn 4.6) 

Equation 4.5 requires the storage of the last sample position of the servo motor 
[PM(X'— 1)1 and the last estimated angular velocity [PM(X— 1)]. The stored value of 
PM(X’~ 1) can be recalculated after 2 samples of position are taken. If the second- 
order model switches from full acceleration to curve following between samples of 
position. Equation 4.5 is not self-correcting until two position samples can be taken 
after switching. To take care of the deficiency, the switching time is detected and the 
present value of the second-order model velocity is stored as PM(X~ 1) to be used in 
the next calculation. 
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FINAL POSITION 
PM1= 1 radian 
PM2= 1 radiain 




INITIAL 

PM1(0)= 

PM2(0)= 



POSITION 
0 radian 
0 radian 



Figure 5.1 Sample Move #1. 




Figure 5.2 Sample Move #2. 
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INITIAL 

PM1(0)= 

PM2(0)= 



POSITION 
0 radian 
0 radian 



I 



Figure 5.3 Sample Move #3. 



VI. THE VOLTAGE SOURCE DRIVE SYSTEM 



A. INTRODUCTION 

The computer adaptive system with voltage source drive was developed in 
Chapter IV. Figure 4.5 illustrates the block diagram of the adaptive system with 
voltage source drive. The DSL/ VS simulation program used in the testing of the 
adaptive system with Voltage Source Drive, is listed in Appendix C. Both adaptive 
models with servo motors are simulated at the same time in the program to assure 
realistic interaction between joint servo motors. 

A sample region was added to obtain the servo motor angular positions at 
discrete time intervals PM(N). The DELS parameter of the DSL VS simulation 
program allows the simulation to enter the sample region every T = 0.25 milliseconds. 
The adaptive algorithm, also included in the sample region, calculates the estimates of 
servo motor angular velocity (PM) and the second-order model gain parameter 
from the sample of the servo motor angular position. At every sampling time, the 
second-order model states P, P are reset with the values of the servo motor position 
and velocity PM, PM and the model gain parameter is updated during the full 
acceleration part. Fixed step integration was used in the simulation to support easy 
hardware implementation. The integration interval was chosen to be 1,5 the sampling 
period. 

B. SIMULATION STUDIES OF THE ADAPTIVE SYSTEM 

Appendix C and D list the DSL, VS simulation programs used in testing the 
adaptive system. The simulation program for the gravity-free environment is included 
in Appendix C. The simulation program in Appendix D includes gravitational torques. 

1. Gravity-free Environment 

The adaptive system without gravitational torques is tested for both step 
position command and time varying position command inputs. Note that on all phase 
plane plots the velocity of the second-order model and the velocity of the servo motor 
are both plotted as ordinates. In general these curves fall on top of each other. .Move 
~l and .Move i=2 are used in this part of the simulation studies. The following results 
are observed; 
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a. Step Position Command Input Used 

Figures 6.1 and 6.2 are the phase plane plot and step response curves for 
the unloaded arm. In the phase plane plot, the second-order angular velocity P and 
the servo motor angular velocity PM are plotted versus servo motor angular position 
PM. Although PM cannot be observed in the actual system, it is available in the 
simulation and is used to check the validity of the algorithm. 

Figure 6.1 shows that the second-order model and servo motor have good 
curve following characteristics for both joints. The step response curves of Figure 6.2 
show the models and servo motors tracking together. Steady state accuracy of the 
order of lOE-4 is observed. The loaded arm was simulated in Figures 6.3 and 6.4 In 
these figures, effects of the reaction torques can be observed. At the beginning of the 
move, upward motion of the L1XK2 creates an interaction torque on JOINTI servo 
motor. Therefore it takes time to overcome the reaction torque and LINK I starts 
accelerating. To avoid this unwanted situation, the .position command for J01NT2 
servo motor is delayed 150 millisecond at the beginning of the move. This is shown in 
Figures 6.5 - 6.8. 

The effects of the delayed input for phase plane trajectories can be 
observed in Figures 6.5 and 6.7. When J01NT2 servo motor starts accelerating, the 
reaction torque causes JOINTI servo motor to decelerate for a while. Meanwhile, 
curve following seems lost but after a while the adaptive system tracks and follows the 
curve until the desired position is reached. Faster over-all response was obtained with 
this strategy also. 

Simulation studies for Move ^2 are shown in Figures 6.9 - 6.16. Figures 
6.9 and 6.11 are good illustrations to point out the importance of the reaction torques 
between joints. Interactive torques gain more importance when servo motors operate 
in opposite directions. In these figures it can be observed, acceleration of the J01NT2 
servo motor helps JOINTI servo motor to speed up quickly. When J01NT2 servo 
motor reaches the curve and starts deceleration, the interaction torque created by 
J01NT2 servo motor opposes JOINTI servo motor acceleration. Ma.ximum angular 
velocity of 3.7 rad/sec and 13.4 rad, sec are observed for motors 1 and 2 respectively. 
Step response curves of Move #2 are shown in Figures 6.10 and 6.12. 

The adaptive system was also simulated for a dilTerent input sequence, ie 
JOINT2 servo motor position command delayed at 0.15 second. The eflects of the 
delayed input can be observed in phase plane plots of Figures 6.13 and 6.15. 



36 



15 




11 




4 




I 



I 



Figure 6.1 Phase Plane Trajector}’ For Move #1 (No Gravity), 
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Figure 6.2 Step Response For Move #1 (No Gravity). 
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Figure 6.3 Phase Plane Trajectory For Move =fl (No Gravity). 
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Figure 6.4 Step Response For Move #1 (No Gravity). 
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Fieure 6,5 Phase Plane Trajecton’ For Move (No Gravitv) 
JOINT2 Servo Motor Input Delayed 0.15 sec. 
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Fisure 6.6 Step Response For Move #1 (No Gravitv) 
JOINT2 Servo Motor Input Delayed 0.15 sec. 
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Figure 6.7 Phase Plane Traiectorv For Move =1 (No Gravitv) 
JOINT2 Servo Motor Input Delayed 0.15 sec. 
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Figure 6.S Step Response For Move #1 (No Gravity) 
JOINT2 Servo \Iotor Input Delayed 0.15 sec. 
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Figure 6.9 Phase Plane Trajectory For Move #2 (No Gravity). 
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Figure 6.10 Step Response For Move #2 (No Gravity). 
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Figure 6.11 Phase Plane Trajector>’ For Move #2 (No Gravity). 
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Figure 6.12 Step Response For Move #2 (No Gravity). 
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Figure 6.13 Phase Plane Traiectorv For Move {'So Gravitv) 
JOINT2 Servo Motor Input Delayed 0.15 sec. 
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Figure 6.14 Step Response For Move #2 (No Gravity) 
JOINT2 Servo Motor Input Delayed 0.15 sec. 
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Figure 6.15 Phase Plane Traiectorv For .Move ??2 (No Gravitv) 
JOINT2 Servo .Motor Input Delayed 0:i5 sec. 
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Fisure 6.16 Step Response For Move #2 (No Gravity) 
JOINT2 Servo .Motor Input Delayed 0.15 sec. 
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At the beginning of Move #2 JOINTl servo motor moves slower while the 
other servo motor stands still. After J01NT2 servo motor starts accelerating, 
appreciable speed-up can be observed in the first servo motor phase plane trajectorv’. 
This speed-up results in passing over the curve a little. After a while the system tracks 
the curve and follows it until the position is reached. Step response curves in Figures 
6.14 and 6.16 show 40 milliseconds slower response time in comparison to the 
simulation results obtained when simultaneous inputs are applied. 
b. Time Varying Position Command Input Used 

To simulate the adaptive system under time varying command, a ramp 
input with tangent of 1 rad/sec and sinusoidal input are used. Simulation results are 
shown in Figures 6.17 - 6.2S. Phase plane trajectories for ramp input are shown in 
Figures 6.17 and 6.20. Good curve following is observed in the figures. J01NT2 servo 
motor velocity builds up and becomes closer to steady-state velocity in a short time 
interval. JOINTl servo motor velocity can't build up as fast as the second servo 
motor because of large inertial and interaction torques. Therefore JOINTl servo 
motor follows the ramp input with larger errors. Figures 6.19 and 6.22 show position 
error for servo motors. Phase plane trajectories for sinusoidal input are shown in 
Figures 6.23 and 6.26. Good curve following is seen in these figures. JOINT2 servo 
motor follows the given input command easily with small errors but JOINTl servo 
motor lags behind the sine input curve because of the reasons mentioned above in the 
case of a ramp input. Position error between commanded position input and actual 
serv'o motor position output is given in Figures 6.25 and 6.28. 

Simulation studies show that the magnitude of the position error depends 
on the sinusoidal position command input magnitude and frequency. The smaller the 
frequency, the smaller the position error that is obtained. For a fixed frequency, the 
smaller magnitude of input gives a smaller position error. 

2. Gravitational Torques Included 

The adaptive system was simulated under gravitational torques. Moves #1. =^2 
and ^3 were used to test the system for the existence of excess interactive torques. 
Both step input and time varying input were used in the simulation studies. 
a. Step Position Command Input is Used 

Figures 6.29 - 6.32 are the phase plane plots and step response curves for 
simultaneously applied step position input commands. Phase plane trajectories show 
that JOINTl servo motor develops negative velocity (in the opposite direction of the 
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Figure 6.17 Phase Plane Trajectory' For Ramp Input (No Gravity). 
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Figure 6. IS Ramp Response (No Gravity). 
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Figure 6.19 Error Between Commanded and Actual Position (No Gravity). 
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Figure 6.20 Phase Plane Trajectory For Ramp Input (No Gravity - Loaded Arm). 
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Figure 6.21 Ramp Response (No Gravity - Loaded Arm). 
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Figure 6.22 Error Between Commanded and Actual Position 
(No Gravity - Loaded Arm). 
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Figure 6.23 Phase Plane Trajectory For Sinusoidal Input 

{No Gravity)! 
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Figure 6.24 Sine Response (No Gravity). 
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Figure 6.25 Error Between Commanded and Actual Position 

(No Gravity). 
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Figure 6.26 Phase Plane Trajectory For Sinusoidal Input 
(No Gravity - Loaded Arm). 
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Figure 6.27 Sine Response (No Gravity - Loaded Arm). 
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Fi 2 ure 6.28 Error Between Commanded and Actual Position 
(No Gravity - Loaded Arm). 
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Figure 6.29 Phase Plane Trajectory’ For Move #1 (With Gravity). 



66 




1.5 



UO 



0.5 




1.5 



1.0 



0,5 



0.0 



• 0.5 



STEP RESPONSE (VCLTftGE SCL«. C3RV.) 



Figure 6.30 Step Response For Move (With Gravity). 
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move) at the beginning of the motion. This negative velocity is caused by gravitational 
torques and interactive torques in the system. It can be observed that gravitational 
torques are at the maximum value when servo motors start moving. .After JOIXTl 
servo motor torque overcomes the disturbance torques, the servo motor starts 
accelerating in the direction of the move, catches and follows the curve until the final 
position is reached. 

In the case of a loaded arm, a larger magnitude of negative velocity is seen 
because of greater gravitational torques. Step response curves are shown in Figures 
6.30 and 6.32. It can be observed that the negative velocity at the beginning of the 
move resulted in JOINTI servo motor moving in the wrong direction initially. One 
might think that the situation at the beginning of the move may bring problems in 
some applications. To avoid this situation. J01NT2 servo motor position command 
was delayed 0.1 second at the beginning. Simulation results are shown in Figures 6.33 
- 6.36. Phase plane trajectories show that the negative velocity at the beginning of the 
move is lost. A zig-zag shape in JOINTI servo motor phase trajectory is seen due to 
reaction torque produced by JOINT2 servo motor. The same overall time response 
was obtained. 

Simulation studies with Move =2 are shown in Figures 6.37 - 6.40. It can 
be seen that both second-order model and servo motor have good curve following 
capabilities and they are on top of each other. Maximum velocities of 6.6 rad. secand 
22 rad. sec are seen for servo motors 1 and 2 respectively. In this sample move, the 
reaction torques increase the servo motor acceleration until the curve is reached. .After 
that, reaction torques oppose the servo motor deceleration along the curve. As a 
result, acceleration time of the move is shorter than deceleration time. Step response 
curves are shown in Figures 6.38 and 6.40. 

Move #3 was used for extensive testing of the system. Phase plane 
trajectories are shown in Figures 6.41 and 6.43. It can be seen at the beginning of the 
move, when servo motors start accelerating, the reaction torque produced by J01NT2 
servo motor keeps JOINTI servo motor from accelerating, even causing it to move in 
the opposite direction to the motion. This situation is clearer in the step response 
curves of Figures 6.42 and 6.44. To avoid the unwanted efiects of the reaction torques 
at the beginning of the motion, step position conunand input to the second servo 
motor was delayed 100 milliseconds. Simulation results are shown in Figures 6.45 - 
6.48. For the unloaded arm. a dilTerent input sequence worked out well. From the 
phase plane plot (Figure 6.45) good curve following can be seen. 
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Figure 6.31 Phase Plane Trajectory For Move =?1 {With Gravity). 
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Figure 6,32 Step Response For Move #1 (With Gravity). 




Figure 6.33 Phase Plane Traiectorv’ For Move =1 (With Gravity) 
JOINT2 Servo Motor Input Delayed 0.10 sec. 
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Figure 6.34 Step Response For Move =1 (With Gravitv) 
JOINT2 Servo Motor Input Delayed 0.10 sec. 



72 



EHtJlI_mQPZSOJ_ EUM-IBQD/SECJ. 



r 




29 

15 




4 




! 




Fisure 6.35 Phase Plane Traiectorv For Move =1 (With Gravitv) 
JOINT2 Servo Motor Input Delayed 0. lu sec. 
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Figure 6,36 Step Response For Move (With Gravity) 
JOINT2 Servo Motor Input Delayed 0,10 sec. 
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Figure 6.37 Phase Plane Trajectorv' For Move (With Gravity). 
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Figure 6.3S Step Response For Move #2 (With Gravity). 
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Figure 6.39 Phase Plane Trajectorv For Move =2 
(With Gravity - Loaded Arm). 
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Figure 6.40 Step, Response For Move #2 
(With Gravity - Loaded Arm). 
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Figure 6.41 Phase Plane Trajector\’ For Move #3 (With Gravity). 
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Figure 6.42 Step Response For Move #3 (With Gravity). 
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Figure 6.43 Phase Plane Trajector>^ For Move ^3 (With Gravity). 



81 






q: 




£l 




0.5 



0.9 



• 0.5 



• 1.0 



• 1.5 



‘ 2.0 



QC 



2.0 



1.5 



1.0 



0.5 



0.0 



• 0-5 



8 

tt: 



Figure 6.44 Step Response For Move t£3 (With Gravity). 
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Figure 6.45 Phase Plane Traiectorv For Move =3 (With Gravitv) 
JOINT2 Servo Motor Input Delayed 0. lO sec. 
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Figure 6.46 Step Response For Move =i3 (With Gravity) 
JOINT2 Servo Motor Input Delayed 0.10 sec. 
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It is also noticed that when JOINT2 sen'o motor starts accelerating, the 
reaction torque produced causes the JOINTl ser\'o motor velocity to go down for a 
while, resulting in passing over the curve and staying there for a certain amount of 
time before it catches up and follows the curve until the desired position is reached. 
From the step response curve (Figure 6.46), 60 milliseconds faster overall response is 
observed. 

Phase plane plots and step response curves for Loaded arm with dilTerent 
input sequences are shown in Figures 6.47 and 6.48. Delayed (100 milliseconds) step 
position input for JOINT2 servo motor causes JOINTl servo motor to overshoot. 
This result implies the importance of the effects of the inertial torques and interaction 
torques on the loaded arm. 

b. Time Varyin" Position Command Input Used 

Ramp input with tangent of 1 rad sec and a sinusoidal input were used to 
test the adaptive system under gravitational torques. Simulation results are shown in 
Figures 6.49 - 6.60. Phase plane trajectories of Figures 6.49 and 6.52 show second- 
order model and servo motor velocities are oscillating around the curve. From Figures 
6.50 and 6.53, it is seen that JOINT2 servo motor follows the ramp input with very 
small errors, while JOINTl servo motor is lagging behind the ramp input with larger 
errors. Figures 6.51 and 6.54 show errors between commanded position input and 
actual position of the servo motors. It can be observed, that the error tends to go to 
zero very slowly. 

Simulation studies with sinusoidal input under gravitational torques are 
shown in Figures 6.55 - 6.60. Good curve following is observed from phase plane 
trajectories. Sine response curves in Figures 6.56 and 6.59, show the second-order 
models and servo motors follow the time varv’ing position command with small errors. 
The error between desired and actual position is shown in Figures 6.57 and 6.60. 
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Fisure 6,47 Phase Plane Traiectorv For Move #3 (With Gravitv) 
JOINT2 Serv'o Motor Input Delayed 0, lO sec. 
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Figure 6.48 Step Response For Move ^3 (With Gravity) 
JOINT2 Ser\'o Motor Input Delayed 0.10 sec. 
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Figure 6.49 Phase Plane Trajectory For Ramp Input (With Gravity). 
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Figure 6.50 Ramp Response (With Gravity). 
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Figure 6.51 Error Between Commanded and Actual Position 

(With Gravity). 
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Figure 6.52 Phase Plane Trajectorv For Rarnp Input 
(With Gravity - Loadecf Arna). 
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Figure 6.53 Ramp Response (With Gravity - Loaded Arm). 
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Fi 2 ure 6.54 Error Between Commanded and Actual Position 
(With Gravity - Loaded Arm). 
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Figure 6.55 Phase Plane Trajectorv For Sinusoidal Input 
(With Gravity). 
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Figure 6.56 Sine Response (With Gravity). 
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Figure 6.57 Error Between Commanded and Actual Position 

(With Gravity). 
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Figure 6.58 Phase Plane Trajectory’ For Sinusoidal Input 
(With Gravity - Loaded Arm). 
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Figure 6.59 Sine Response (With Gravity - Loaded Arm). 
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Fiaure 6.60 Error Between Commanded and Actual Position 
(With Gravity - Loaded Arm). 
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VII. THE CURRENT SOURCE DRIVE SYSTEM 



A. DEVELOPMENT OF THE CURRENT SOURCE DRIVE SYSTEM 

In a DC servo motor; to achieve constant acceleration, the armature circuit 
current must build up quickly to a constant value and provide constant torque. This is 
important for the second-order model to control the servo motor. A current source 
drive system can be used to accomplish this. 

It can be shown [Ref. 2] that the transfer functions for a permanent magnet DC 
servo motor, when the viscous friction of the motor and load is small, are: 



6(s) ^ 

V(s) s[(R + sL)Js + KyKj.] 



(eqn 7.1 ) 



I(s) _ Js 

V(s) ~ [(R + sL)Js ^ KyKJ 



(eqn 7.2) 



where 

0 = Angular position of the shaft 
V = Applied d-c voltage 

1 = Armature current 
Ky_ Back emf constant 
Kj_ Torque constant 

J = Total inertia 
R = Armature resistance 
L = Armature inductance 

The transfer function block diagram of a Current Source Drive is shown [Ref. 1] in 
Figure 7.1. 

If the amplifier gain is chosen as such that 



KA > > 1 



(eqn 7.3) 



then the equivalent transfer function of the amplifcr can be written as; 



100 



V 



(eqn 7.4) 



[(R^sL)Js ^ 
Va Ts 



The equivalent transfer function becomes: 



6(s) _ K^/J 



(eqn 7.5) 



Thus if the amplifier gain is carefully chosen, the ideal motor of the second-order 
model will be approximated by the system and the effect of the back emf and armature 
inductance will become negligible. 

Simulation block diagram of the current source drive system is shown in Figure 
7.2. Rj was chosen to be 0.1 to minimize the influence on the electrical pole, inversely 
Kr was set at 10.0. The amplifier gain (KA) and saturation limit (VS.ATA) were 
chosen by trial and error. These values were found: 

• Without gravitational toraues 
KA1= 150. VS.ATA1= 100 
KA2 = 75. VS.ATA2 = 50 

• Gravitational toraues are included 
KAl = 550, VS.ATAl = 300 
KA2 = 175. VSATA2 = 150 
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Figure 7.1 Transfer Function Block Diagram of 
the Current Source Drive. 
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Figure 7.2 Block Diagram of the Current Source Drive System. 
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B. SIMULATION STUDIES OF THE ADAPTIVE SYSTEM. 

DSL/ VS simulation programs used in simulation of the adaptive system with 
Current Source Drive are listed in Appendix E and F. The simulation program listed 
in Appendix E is used for studies under a gravity-free environment. Appendix F lists 
the simulation program which includes gravitational torques. Again note that although 
PM cannot be observed in the actual system, it is available in the simulation and is 
used to check, the validity of the algorithm. 

1. Gravity-Free Environment 

Step position command and time varv’ing position command inputs are used 
to test the adaptive system under a gravity-free environment {No Gravitional Torques). 
Sample motions, Move #1 and Move #2 are used in the simulation studies of this part. 
Note that on all figures of Phase Plane Trajectories the angular velocity of the second- 
order model (P) and the angular velocity of the servo motor (PM) are both plotted as 
ordinates. The second-order model angular position (P) and the servo motor angular 
position (PM) are both plotted as ordinates in the time response plots. In general 
these curves fall on top of each other. 

a. Step Position Command Input Used 

Phase plane plot and step response curves for the unloaded arm under 
Move #1 are shown in Figures 7.3 and 7.4. In the phase plane plot PM and P are 
plotted versus PM. The step response curve shows P and PM are plotted versus time. 

Good curv'e following characteristics are seen in Figure 7.3. Maximum 
angular velocity of 3.17 rad,' sec and 12 rad/ sec are observed for servo motors 1 and 2 
respectively. The step response curves of Figure 7.4 show the second-order models and 
servo motors tracking together. Also much faster time response was observed for 
JOINT2 servo motor due to smaller inertia and supporting reaction torques in the 
direction of motion. Steady state accuracy of the order of lOE-4 is observed. 

The simulation results for the loaded arm are shown in Figures 7.5 and 7.6. 
The effects of the reaction torques can be observed in these figures. At the beginning 
of the move, upward motion of the L1NK2 produces an interaction torque on JOINTI 
servo motor. This reaction torque opposes JOINTI servo motor acceleration in the 
direction of motion. Therefore it takes some time to overcome the reaction torque, 
and LINKl starts accelerating. To avoid this situation, J01NT2 servo motor position 
command input is delayed 150 milliseconds at the beginning of the Move s^l. 
Simulation results are shown in Figures 7.7 - 7.10. 
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Figure 7.3 Phase Plane Trajectory’ For Move #1 (No Gravity). 
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Figure 7.4 Step Response For Move ,^1 (No Gravity). 



Pi (Rf^oj P2 mnor 



r 




Figure 7.5 Phase Plane Trajectory For Move #1 (No Gravity). 
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Figure 7.6 Step Response For Move (No Gravity). 
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Fisure 7.7 Phase Plane Traiectorv For Move =1 (No Gravity) 
JOINT2 Servo Motor Input Delayed 0.15 sec.. 
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Fiaure 7.8 Step Response For Move =1 (No Gravitv) 
~ JOINT2 Servo .\fotor Input Delayed 0.15 sec.. 
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Figure 7.9 Phase Plane Trajectorv For Move =1 (No Gravitv) 
JOINT2 Servo Motor Input Delayed 0.15 sec.. 
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Fisure 7.10 Step Response For Move =1 (No Gravity) 
JOINT2 Servo Motor Input Delayed 0.15 sec.. 
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The effects of the delayed JOINT2 servo motor can be observed in the 
phase plane trajectories of Figures 7.7 and 7.9. At the beginning, JOINTl servo motor 
starts accelerating \vhile JOINT2 servo motor stays still. When JOIXT2 servo motor 
starts accelerating, the reaction torque produced by JOIXT2 servo motor causes 
JOINTl servo motor to decelerate for a while. Meanwhile, curve following seems lost, 
but after a while the adaptive system tracks and follow’s the curve until the final 
position. In the loaded arm case, 7% overshoot was observed for JOINT2 servo 
motor. This can be prevented by lowering the curve gain constant. Faster overall 
response was obtained with delayed input. 

Figures 7.11 - 7.18 show simulation results for Move #2. Figures 7.11 and 
7.13 are good examples to show the effects of the interaction torques on the servo 
motors. At the beginning of the move, the reaction torques produced by accelerations 
of the servo motors increase each other's acceleration in the direction of the move. 
Therefore, servo motors reach maximum angular velocities in a short time. When 
JOINT2 servo motor reaches the curve and starts braking, the interaction torque 
produced by JOINT2 servo motor opposes JOINTl servo motor acceleration in the 
direction of the motion. Step response curves of Move #2 are shown in Figures 7.12 
and 7.14. It can be observed, in the case of loaded arm, JOINT2 servo motor 
overshoots and undershoots before it reaches the steady-state. This is due to the 
reaction torque created by JOINTl servo motor. 

To take care of this deficiency, JOINT2 servo motor curve gain constant 
was lowered to 0.5. Simulation results are shown in Figures 7.15 - 7.18. Phase plane 
trajectories show' that smaller, average velocities are obtained for JOINT2 servo motor 
w'ith a lowered deceleration curve. Good curv'e following characteristics are observed. 
Step response curves show almost no overshoot but longer time response for JOINT2 
servo motor. This can be thought of as a price to be paid to get no overshoot. But if 
we think over-all time response depends on JOINTl sen’o motor, then this time 
increase response of JOINT2 servo motor doesn't bring on an additional time increase 
to the system. 

b. Time Varying Position Command Input Used 

The adaptive system under time varying command input was simulated by 
using a ramp input with tangent of 1 rad; sec and a sinusoidal input. Simulation results 
are shown in Figures 7.19 - 7.30. Phase plane trajectories of Figures 7.19 and 7.22 
show” JOINT2 servo motor velocity builds up quickly and reaches the steady-state 
velocity in a short time. 
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Figure 7.11 Phase Plane Trajectory' For Move #2 (No Gravity). 
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Figure 7.12 Step Response For Move ^2 (No Gravity). 
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Figure 7.13 Phase Plane Trajectory For Move #2 (No Gravity). 
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Figure 7.14 Step Response For Move #2 (No Gravity). 
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Fieure 7.15 Phase Plane Trajectory For Move #2 (No Gravity) 
JOINT2 Servo Motor Curve Gain Constant Lowered to 0.5. 
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Fisure 7.16 Step Response For Move #2 (No Gravity) ^ 
JOI?\T2 Servo Motor Curve Gain Constant Lowered to O.o. 
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Fisure 7.17 Phase Plane Trajecton/^ For Move ^2 (No Gravity) 
JOINT2 Servo Motor Curve Gain Constant Lowered to 0,5/ 
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Fisure 7.18 Step Response For Move #2 (No Gravit\0 
JOINT2 Servo Motor Curve Gain Constant Lowered to"0.5. 



120 



1,51 



1.5 



1.^ 




i.a 



S| 



§ 

B 



0.5- 



9.5 



0 . 0 - 



— 1 1 1 ! 1 h 

*100 0 100 ^00 500 400 SOO 





1.5 



1.8 



0.5 



0.8 



CJ 



•0.5 i 



PmSE PLftNE (CURRENT SCUR. DRV. J 



Figure 7.19 Phase Plane Trajector>' For Ramp Input (No Gravity). 
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Figure 7.20 Ramp Response (No Gravity). 



122 



1291 



12 * 




Figure 7.21 Error Between Commanded and Actual Position (No Gravity). 
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The second-order model and servo motor velocity of J01NT2 shudders 
around the curve until the desired position is reached. Large inertial torques and 
interaction torques prevent JOINTI servo motor velocity to build up as fast as 
J01NT2 servo motor veloticy. It takes more time for JOINTI servo motor velocity to 
reach steady-state velocity. Therefore, JOINTI servo motor follows the ramp input 
with larger errors. Errors between commanded position input and actual position 
output of servo motor are shown in Figures 7.21 and 7.24. 

Phase plane plots of simulation studies for sinusoidal input are shown in 
Figures 7.25 and 7.28. Good curve following capabilities are seen in phase plane 
trajectories. From Figures 7.26 and 7.29, it can be observed that J01NT2 servo motor 
follows the sinusoidal input easily while JOINTI servo motor lags behind the sine 
input curve. The reasons for this we mentioned above in the case of a ramp input. 
Position errors for servo motors are shown in Figures 7.27 and 7.30. Again simulation 
studies show that the smaller the frequency of the sinusoidal position command input, 
the smaller the position error that is obtained. Also, for a fi.xed frequency, the smaller 
magnitude of input curve gives a smaller error. 

2. Gravitational Torques Included 

To simulate the adaptive system under gravitational torques. Moves #1, #2 

and #3 were used. Again note that on all figures of phase plots, the second-order 

• • 

model velocity (P) and servo motor velocity (PM) are both plotted as ordinates versus 
servo motor angular position (PM). In time response curves, the second-order model 
angular position (PM) are both plotted as ordinates. In general, these curves fall on 
top of each other. In the simulation studies, both step and time varying position 
command input were used. 

a. Step Position Command Input Used 

Phase plane plots and step response curves for Move #1 are shown in 
Figures 7.31 - 7.34. In phase plane trajectories of Figures 7.31 and 7.33, it can be 
observed that gravitational torques and reaction torque produced by JOINT2 servo 
motor causes JOINTI servo motor to move in the opposite direction of the move. 
Note that gravitational torques are at the maximum value at the beginning of the 
move. It takes a while to overcome the gravitational and reaction torques, after that 
the JOINTI servo motor stars accelerating until it catches and follows the curve till the 
desired position is reached. 
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Figure 7,22 Phase Plane Trajectory For Ramp Input (No Gravity - Loaded Arm;. 
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Figure 7.23 Ramp Response (No Gravity - Loaded Arm). 
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Figure 7.24 Error Between Commanded and Actual Position 
(No Gravity - Loaded Arm). 
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Figure 7.25 Phase Plane Traiectorv For Sinusoidal Input 

(No Gravity)! 



12S 




1.5 



1.Q 



0.5 



0.0 

- 0.5 

- 1.0 

- 1.5 



a 

£21 

04 



1.5 



1.0 



0.5 



0.0 



Figure 7.26 Sine Response (No Gravity). 







Figure 7.27 Error Between Commanded and Actual Position 

(No Gravity). 
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Figure 7.28 Phase Plane Trajectory For Sinusoidal Input 
(No Gravity - Loaded Arm). 
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Figure 7.29 Sine Response (No Gravity - Loaded Arm). 
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Figure 7.30 Error Between Commanded and Actual Position 
(No Gravity - Loaded Arm). 
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Figure 7.31 Phase Plane Trajector\^ For Move ^1 (With Gravity). 
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Figure 7.32 Step Response For Move #1 (With Gravity). 
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Figure 7.33 Phase Plane Trajectory For Move ffl (With Gravity). 
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Figure 7.34 Step Response For Move #1 (With Gravity). 
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The phase plane plot for loaded arm (Figure 7.33) shows that the 
magnitude of the negative velocity was increased for JO INTI servo motor. When 
JOINT2 servo motor catches, the curve starts deceleration, JOINTl servo motor 
velocity starts increasing veiy' rapidly until JOINT2 servo motor reaches the steady- 
state. If we think that the opposite movement of the JOINTl servo motor has to be 
avoided, then a dilTerent input sequence may be used to accomplish this. For this 
purpose, JOINT2 servo position command input was delayed 200 milliseconds at the 
beginning. Simulation results are shown in Figures 7.35 and 7.36. From phase plane 
trajectories, it can be observed that JOINTl servo motor can start accelerating without 
going in the wrong direction while JOINT2 servo motor stands still. A zig-zag shape is 
seen in JOINTl servo motor trajectory due to the reaction torque produced by 
JOINT2 servo motor after it starts accelerating. .Also, from step response (Figure 
7.36), 10‘^/o overshoot is observed for JOINT2 servo motor due to the reaction torque 
produced by JOINTl servo motor. The same overall time response was obtained. 

Simulation results for Move ^2 under simultaneous applied inputs, are 
shown in Figures 7.37 - 7.40. Phase plane trajectories (Figure 7.37) show that both 
second-order model and servo motor have good curve following capabilities with 
maximum velocities of 6.5 rad, sec and 27.6 rad sec for the loaded arm. Figure 7.39 
show that JOINT2 servo motor reaches steady-state after it overshoots twice and 
undershoots once. This is clearer in step response curves of Figures 7.40. A different 
input sequence was used to avoid the JOINT2 servo motor overshoot. Therefore, 
JOINT2 servo motor position command input was delayed 100 milliseconds. The 
purpose of this delay is to minintize the effect of the reaction torque produced by 
JOINTl servo motor on JOINT2 servo motor. With delayed input JOINT2 servo 
motor reaches steady-state without overshoot, while JOINTl servo motor decelerates. 
This deceleration provides additional braking torque on the J01NT2 servo motor. 
Simulation results are shown in Figures 7.41 - 7.44. The same overall time response 
was obtained. 

Move #3 was used for additional testing of the system under different 
loading conditions. Simulation results are shown in Figures 7.45 - 7.48. Phase plane 
trajectories show that, at the beginning of the move, the reaction torque produced by 
acceleration of JOINT2 servo motpr prevents JOINTl servo motor from accelerating 
for a while. On the contrary the reaction torques do not work in favor of the servo 
motors for Move #3. 
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Figure 7.35 Phase Plane Traiectorv For Move ^=1 (With Gravity) 
JOINT2 Servo Motor Input Delayed 6.20 sec.. 
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Figure 7.36 Step Response For Move ^^1 (With Gravity) 
JOINT2 Ser\’o Motor Input Delayed 0.20 sec.. 
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Figure 7.37 Phase Plane Trajectory For .VIove #2 (With Gravity). 
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Figure 7.38 Step Response For Move ~2 (With Gravity). 
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Figure 7.39 Phase Plane Trajectorv For Move #2 
(With Gravity - Loaded Arm). 
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Figure 7.40 Step Response For Move #2 
(With Gravity - Loaded Arm). 
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Figure 7.41 Phase Plane Trajectorv’ For Move (With Gravitv) 
JOINT2 Servo Motor Input Delayed 0.10 sec.. 
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Figure 7.42 Step Response For Move #2 (With Gravity) 
JOINT2 Servo Motor Input Delayed 0.10 sec.. 
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Fisure 7.43 Phase Plane Trajectorv For Move =2 (With Gravitv) 
JOIXT2 Servo Motor Input Delayed 0. 10 sec.. 
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Figure 7.44 Step Response For Move #2 (With Gravitv) 
JOINT2 Servo Motor Input Delayed 0.10 sec.. 
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Figure 7 45 Phase Plane Trajector>' For Move #3 (With Gravity)- 
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Figure 7.46 Step Response For Move #3 (With Gravity). 
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Figure 7.47 Phase Plane Trajectory For .Move #3 (With Gravity). 
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Figure 7.48 Step Response For Move #3 (With Gravity). 
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From phase plane trajectories 6.9 rad/sec and 21.3 rad, sec, maximum 
velocities are seen for servo motors 1 and 2 respectively. Good curve following 
capabilities for both the second-order model and servo motor are seen and they are on 
top of each other along the move. 

Z». Time Varying Position Command Input Used 

To simulate the adaptive system under gravitational torques, a ramp input 
with a tangent of 1 rad, 'sec and a sinusoidal input were used. Simulation results are 
shown in Figures 7.49 - 7.60. 

Phase plane trajectories of Figures 7.49 and 7.52 show that at the beginning 
of the move, the second-order model and servo motor velocities increase very rapidly to 
reach the steady-state value. It can be observed that this process takes a much shorter 
time for JOINT2 servo motor in comparison to JOINT 1 servo motor. This can be 
thought to be the result of the reaction produced by JOINT2 servo motor. The 
second-order model and servo motor angular velocities shudder around the curve to 
follow it. Ramp response curves of Figures 7.50 and 7.53 show the J01NT2 servo 
motor is able to follow the ramp input easily with ver\’ small errors. Meanwhile. 
JOINT2 servo motor is lagging behind the ramp input because of the reasons 
mentioned above. Position error between commanded and actual angular position of 
the servo motors are shown in Figures 7.51 and 7.54. In the case of a loaded arm, the 
error reaches its maximum value at the beginning of the move, after that it decreases 
exponentially. 

Figures 7.55 - 7.60 are the simulation results obtained with sinusoidal 
position input. Phase plane plots show that good curve following capabilities are 
observed. More shuddering around the curve is seen for JOINT2 servo motor. Sine 
response curves of Figures 7.56 and 7.59 show that both servo motors follow the 
commanded input. The position error between the desired angular position and actual 
servo motor angular position is shown in Figures 7.57 and 7.60. 
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Figure 7.49 Phase Plane Trajectory For Ramp Input (With Gravity). 







1.5 



1.0 



0.5 



O.Q 



<\j 



2.0 



1.5 



1.0 



0.5 



0.0 



RflflP RESPONSE (CURRENT SOUR. DRV.) 



Figure 7.50 Ramp Response (With Gravity). 
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Figure 7.51 Error Between Commanded and Actual Position 

(With Gravity). 
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Fisure 7.52 Phase Plane Trajectory For Ramp Input 
(With Gravity - Loadea Arm). 
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Figure 7.53 Ramp Response (With Gravity - Loaded Arm). 
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Figure 7.54 Error Between Commanded and Actual Position 
{With Gravity - Loaded Arm). 
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Figure 7.55 Phase Plane Traiectorv For Sinusoidal Input 
(With Gravity). 



160 




1.5 



1.1 



0.5 



0.0 



- 0.5 

- 1.0 

- 1.5 



i 



£XL 






1.5 



1.0 




Figure 7.56 Sine Response (With Gravity). 



161 



E RIJIIU Rf fJi 




ERROR VS TIME rCURRENT SOUR. DRV. J 



Figure 7.57 Error Between Commanded and Actual Position 

(With Gravity). 
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Figure 7.58 Phase Plane Trajector>’ For Sinusoidal Input 
(With Gravity - Loaded Arm). 
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Figure 7.59 Sine Response (With Gravity - Loaded Arm). 
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Fiaure 7.60 Error Between Commanded and .Actual Position 
(With Gravity - Loaded Arm). 
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VIII. CONCLUSIONS/AREAS FOR FURTHER STUDY 



As a result of the simulation studies done in this thesis, the near minimum time 
positioning of a two-link robot manipulator with an adaptive computer model seems 
feasible. Three sample moves were used to remove as many nonlinearities as necessary 
to test the adaptive system under large parameter v'ariations. In these nonlinearities, 
coupling inertia, centripetal forces, actuator dynamics and gravitational torques were 
included. 

As observed, servo motors overshoot for some of the moves. These conditions 
were overcome by offering alternatives such as lowering curve gain constant or using a 
different input sequence. 

In this study, the direct-drive arm was used as a model. It was observed that in 
the case of the direct driv'e arm, the effects of the disturbance torques caused by the 
coupling inertia between links, and the centripetal, coriolis and gravitational forces on 
the robot arm, are larger. To overcome the disturbance torques, the servo motor has 
to apply larger torques. This requires a servo motor with high torque constant and 
high armature current capabilities. As an alternative solution to this, a gear train with 
a gear ratio of N may be used. Thus the effects of the disturbance torques were 
reduced by N. Of course, the price of this alternative is a slower system. It is up to 
the user to decide which is the best trade-off for a particular application. 

Simulation results show that steadv-state accuracv of the order of lOE-4 was 

✓ ✓ 

observed for the step position command input. This accuracy may not be enough for 
some applications. Therefore a further study area arises here to position the L1NK2 
tip more accurately. Some alternative ways of doing this may be considered as; 

1. Replace the curve with a linear cornpensator and remove the tachometer 
feedback within the adaptive moder 

2. Replace the curve in the model with a gain block and modify the 
fachofnefer feedback gain. 

In the modelling of the arm, a rigid body was assumed. Another area for further 
study arises when a lightweight, flexible structure is used to build the robot arm. The 
effects of the mechanical resonance is due to the flexible structure on the adaptive 
system to be studied. 
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In this thesis delayed steps were used to prevent undesired motions. To use this 
technique efiectively in a robot, the magnitude of such step, and the required delay 
time must be predetermined by on-line calculation using some sort of algorithm which 
considers the initial oriantations of the arm. the commanded direction of the step. etc.. 
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APPENDIX A 

DSL PROGRAM FOR THE SECOND-ORDER MODEL SIMULATION 



TITLE DSL PROGRAM OF THE SIMULATION MODEL 
PARAM K1=0.60,K2=10000. , KM=4 , 0 , VSAT=150 . ,K= 
PARAM REF=1.0 
INITIAL 

A=SQRT(2.*KM*VSAT) 

XDOT=0.0 

DYNAMIC 

R=REF*STEP(0.0) 

IF?E . LT . 0 , 0 )XD0T=-A*K1*S0RT (ABS (E ) ) 
I F ( E . GE . 0 . 0 ) XDOT=A*Kl *SQRT ( E ) 
DERIVATIVE 

XDOTE=XDOT-KPDOT 

KPDOT=PDOT*K 

V=LIMIT(-VSAT,VSAT,K2*XDOTE) 

?DDOT=KM’^V 

PDOT=INTGRL (0 . 0 ,PDD0T) 
P=INTGRL(0.0,PDOT) 

TERMINAL 
METHOD RKSFX 

CONTRL FINTIM=0. 15000, DELT=0. 00005 
PRINT 0.01,P,PDOT,XDOT,V,XDOTE 
SAVE (G1)0. 00005, P,XD0T,PD0T 
SAVE (G3)0. 00005, P, REF 

GRAPH(G1/G1,DE=TEK613,P0=1, .5) P(LE=6.5,UN= 
XDOT(LE=8,NI=8,LO=0. , UN= ' RAD/SEC ' , 
PDOT (LE=8 ,NI=S , LO=0 . 0 , UN= ' RAD/SEC ' 
GRAPH(G3/G3,DE=TEK618,PO=l, .5) TIME(LE=6.5 
P (LE=8 , NI=8 , LO=0 , UN= ' RAD ' , SC=0 . 25 ) 
REF ( LE=8 , NI=3 , L0=0 , SC=0 . 25 , AX=OMIT 
LABEL (Gl) PHASE PLANE 
LABEL (Gl) XDOT,PDOT VS P 
LABEL (G3) STEP RESPONSE 
LABEL (G3) P VS TIME 
END 
STOP 



= ' RAD ' ) 

SC=$AR) 

,PO=6.5,SC=$AR). 
,UN=' SECONDS' ) 

r” 
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APPENDIX B 



derivation of mathematical model for the TWO- 

DEGREES-OF-FREEDOM PLANAR ROBOT ARM 



The Lagrangian, L = K ~ V of the system is: 



L = 1 + 2{m2cl2- + Jj^2)®2" 

+ L 2m2(dj“ + d2“+ 2d[d2COS0T)0 

+ m-)(d^“ + djd->cos02)0|0-) 

— [(nij + m^)dJsin 0 J + m-,d->sin (0 j -i- 02 )]g 



(eqn B. 1) 



Taking partial derivatives of Equation B.l with respect to 0j and 02 



dL 

C01 



= - (nij -i-m 2 )gdjcos 0 j -m-)gd')Cos(0j + 0-^) 



(eqn B.2) 



t7 L * 'y * * 

= - (ni')djd-)Sin0i)0 - (m-)djdosin02)0 J02 



" - m2gd:)Cos{0 j + 02) 



(eqn B.3) 



« « 



Taking partial derivatives of Equation B.l with respect to 0j and 02 •’ 



oL 

C01 



-r m2(dj^ + d2" + 2djd2cos02)0| 
+ m2(d2" + djd^cos02)6-> 



(eqn B.4) 



= (ni2d2^ + 



(eqn B.5) 



Taking derivatives of Equations B.4 and B.5 with respect to time: 



d /dL 



dt \59j^ 



0 7 * * 

(mjdj“ + + ni2'Ji"^ 4 - m2d2‘‘ + 2m-)djd-)Cos02)0 j 

+ (m^d2 + m2djd2Cos0^)0-> — (2m2d^d2sin0->)0 j02 
- (m-)djd')Sin0-))0-)" 



(eqn B.6) 
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(eqn B.7) 




I — (^2*^2 ” ^ djd2cos02)6| 
— (m-)d|d')sin0-))6j6-) 



Substitude Equations B.2 - B.7 into Equation 3.1, obtain: 

Tj = [(nij + m2)dj^ + m->d 9 “ + Jj^j -!-2m-)djd2COS02]0j 

+ m2(d2" + djd2COS02)6-> ~ (2m-)d jd2sin0->)0j02 

* 

— (m-)djd-)Sin0o)0T" + (mj -i-m-))gdjcos0 j 
+ m2gd^cos(0^ + 0-)) 



T2 = ''' m-)(d2“ + djd2cos02)0| 

• * • • 

- (m-)djd-)SinOT)0 j0T + (m-»djd-)Sin0-,;0^02 

** ~ • 7 ** 

+ (ni2d jd2sin0->)0 j“ + m2gd->cos{0j + O2) 



When we arrange the terms in Equations B.S and B.9, we obtain Equation 



(eqn B.S) 



(eqn B.9) 



3.1. 
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APPENDIX C 



DSL PROGRAM FOR VOLTAGE SOURCE DRIVE (NO 
GRAVITATIONAL TORQUES) 



TITLE SIMULATION PROGRAM OF THE ADAPTIVE MODEL (VOLTAGE SOURCE DRIVE) 
TITLE MO GRAVITATIONAL TORQUES 

PARAM K1=0.60,K2=10000. ,KM1=0.17 ,KM2=4 . 00 , VSAT1=100 . , K=1 . 0 , T=0 . 00025 
PARAM G=386.4,H1=0.248,M2=0.082,KT1=14.4,KT2=14.4,D1=15. ,D2=10 . 

PARAM J1=0 .033 , J2=0 . 033 , KV1=0 . 1012 , KV2=0 . 1012 , R1=0 .91 , R2=0 . 91 , K3=0 . 6 
PARAM REF1=1 . 0 , REF2=1 . 0 , L=0 . 0001 , BM1=0 . 0429 , BM2=0 . 0429 , VSAT2=50 . 
INTGER N1 , N2 , FLAGl , FLAG2 
INITIAL 

N1=0 

FLAG 1=0 

N2=0 

FLAG2=0 

P1=0.0 

P2=0.0 

P1DOT=0. 

P2DOT=0. 

P1DDT=0. 

P2DDT=0. 

PM1=0.0 
PM2=0.0 
PM1DT=0 . 

PM2DT=0 . 

PM1DDT=0 . 

PM2DDT=0 . 

X1DOT=0. 

X2DOT=0 . 

TL1=0. 

TL2=0. 

TL11=0. 

TL22=0. 

PP1=L/R1 

PP2=L/R2 

TM1=0. 

TM2=0 . 

IM1=0. 

IM2=0 . 

A1=S0RT(2.’^KM1*VSAT1) 

A2=S6RT ( 2 . *KM2*V5AT2 ) 

DERIVATIVE 

RR1=REF1’^STEP(0.0) 

RR2=REF2*STEP(0.0) 

RRD2=TRANSP ( 50 , 0 . 0 , 0 . 1 0 , RR2 ) 

E1=RR1-P1 

E2=RRD2-P2 

NO SORT 

D11 = (M1+M2)*(D1**2)+M2*(D2**2)+2*M2*D1=^D2*C0S(PM2) 
D12=M2*(D2^*2)+M2*Dl’^D2’"COS(PM2) 

D22=M2*(D2**2) 

D122=-M2’^D1*D2=^SIN(PM2) 

D211=D122 

D112=D211 

D121=D112 

D212=D121 

D221=D212 

PMT=PMH-PM2 

* G1 = (M1+M2)’^G*D1*C0S(PM1)+M2"G*D2*C0S(PMT) 

G2=M2*G*D2^COS(PMT) 

TL1=D12^PM2DDT+D122*PM2DT**2+2*D112*PM1DT*PM2DT 

TL2=D12*PM1DDT-D211’^PM1DT**2 

JTOTl=Jl+Dll 
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JTOT2=J2+D22 

IF(E1.LT.O.O)X1DOT=-A1*K1*SQRT(ABS(E1) ) 
IF(E1.GE.O.O)X1DOT=A1’^K1^SORT(E1) 
IF(E2.LT.O.O)X2DOT=-A2’^K3’^SQRT(ABS(E2) ) 
IF(E2.GE.O.O)X2DOT=A2’^K3’^SQRT(E2) 

SORT 

X1D0TE=X1D0T-K?1D0T 

KP1D0T=P1D0T*K 

V1=LIMIT(-VSAT1,VSAT1,K2*X1D0TE) 

MOSORT 

IF(FLAGl.EQ.l)GO TO 5 

IF(V1.LT.VSAT1 .AND. TIME. GT.O. 00005 )FLAG1=1 
NSW1=N1 

5 CONTINUE 
SORT 

P1DDT=KM1*V1 
P1D0T=INTGRL (0.0, PIDDT ) 

P1=INTGRL(0.0,P1DOT) 

VP1=V1-(KV1*PM1DT) 

IM1=REALPL (0 . 0 , PPl , VPl/Rl ) 

TM1=KT1*IM1 

TNET1=TM1-PM1DT’^BM1-TL1 
PM1DDT= ( 1 . / JTOTl ) *TNET1 
PM1DT=INTGRL(0 . 0 , PHIDDT) 

PM1=INTGRL( 0 . 0 , PMIDT ) 

X2DOTE=X2DOT-KP2DOT 

KP2DOT=P2DOT^K 

V2=LIMIT ( - VSAT2 , VSAT2 , K2 =^X2DOTE ) 

NOSORT 

IF(FLAG2.EQ.l)GO TO 6 

IF ( V2 . LT . VSAT2 . AND .TIME . GT . 0 . 0000 5 ) FLAG2=1 
MSW2=N2 

6 CONTINUE 
SORT 

P2DDT=KM2*V2 
P2DOT=INTGRL (0.0, P2DDT) 

P2=INTGRL(0.0,P2DOT) 

VP2=V2-(KV2=^PM2DT) 

IM2=REALPL (0.0, PP2 , VP2/R2 ) 

TH2=KT2«IM2 

TNET2=TM2-PM2DT*BM2-TL2 
PM2DDT= ( 1 . / JTOT2 ) ^TNET2 
PM2DT=INTGRL(0.0,PM2DDT) 

PM2=INTGRL ( 0 . 0 , PM2DT ) 

SAMPLE 

NOSORT 

IF(M2.EQ.O)GO TO 21 
IF(Nr.EQ.O)GO TO 20 
P2=PM2 
P1=PM1 

KS2=ABS ( 2 . *PM2/ ( V2* ( (N2*T) **2 ) ) ) 

KS1=ABS(2.’='PM1/(V1*( (N1*T)**2) ) ) 

IF(FLAG2.EQ.0)KH2=KS2 

IF(FLAG1.EQ.0)KM1=KS1 

IF (N2 . GE . 2 T PM2DTL= ( PM2 -PM22L ) / ( 2 . *T ) 

IF(N1.GE.2)PM1DTL=(PM1-PM12L)/(2.*T) 

IF(FLAG2.EO.O)P2DOT=(2.*( (PM2-PM2LST)/T))-PM2DTL 

IF(FLAG1.EQ.O)P1DOT=(2.*( (PM1-PM1LST)/Tn-PM1DTL 

IF(N2.EQ.NSW2.AND.FLAG2.EQ.l)GO TO 21 

if(ni.eq.nswi.and.flagi.eq.i)go to 20 

IF ( FLAG2 . EQ . 1 ) P2DOT= ( 2 . * ( (PM2 -PM2LST ) /T ) ) -PM2DTL 
IFfFLAGl .EQ.1)P1D0T=(2.*( (PM1-PM1LST)/T) ) -PMIDTL 
21 N2=N2+1 

20 N1=N1+1 

PM2DTL=P2DOT 

PM1DTL=P1D0T 

P2DOT=PM2DTL 

PM22L=PM2LST 

PM12L=PM1LST 
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?M2LST=FM2 

?M1LST=PM1 

ER1=ABS(RR1-PM1) 

* ER2=ABS(RR2-PM2) 

SORT 

TERMINAL 

FINISH PM1=1.00 
METHOD RKSFX 

CONTRL ?INTIM=0 .70000 , DELT=0 . 00005 , DELS= . 00025 

PRINT 0.010, XIDOT , PIDOT , PMIDT , X2DOT , P2DOT , PM2DT , PMl , RRl , PM2 , RR2 , TMl , TM2 
SAVE (SI) 0.01, XIDOT, PIDOT, PMIDT, X2DOT,P2DOT,PM2DT, PMl ,PM2 
S.AVE (S3) 0.01, PI ,PM1 ,REF1 ,P2,PM2,REF2,RR1 ,RRD2 

GRAPH (L1/S1,DE=TEK618,P0=1, .5) PMl (LE=6 . 0 , UN= ' RAD ' ,LO=-0 , 10 ,SC=0 . 10 , , . . 

NI = 12) ,?1DOT(LE=4,NI=4,LO=-2.0,UN='R.AD/SEC , SC=2 . 0 , PO=6 . 0 ) , . . . 
X1DOT(LE=4,MI=4,LO=-2.0,SC=2.0,UN=’R.AD/SEC' ,AX=OMIT) , . . . 

PMIDT (LE=4,NI=4,L0=- 2. 0,UN=' RAD/SEC ' ,SC=2.0) 

GRAPH (L2/Sl,DE=TEK613,OV,PO=l,5) PM2 (LE=6 . 0 , UN= ' RAD ' ,LO=-0.10, . . . 

SC=0.10,MI=12) , P2DOT(LE=4,NI=4,LO=0.0,UN='RAD/SEC ,SC=5.0, , . . 
PO=6, ) ,X2DOT(LE=4,MI=4,LO=0.0,UM=' RAD/SEC ' , SC=5 . 0 ,AX=0MIT) , . . . 
PM2DT ( LE=4 , NI=4 , L0=0 . 0 , UN= ' RAD/SEC ' , SC=5 . 0 ) 

GRAPH (L3/S3,DE-TEK618,PO=l, .5) TIME (LE=6 . 0 , UN= ' SECONDS '),... 
Pl(LE=4,NI=4,LO=-.5,UN='RAD' , SC=0 . 50 , PO=6 . 0 ) , . . . 
?M1(LE=4,NI=4,L0=-.5,UN='RAD' ,SC=0.50) , . . . 

RRl ( LE=4 , MI=4 , LO=- . 5 , SC=0 . 50 , AX=OMIT ) 

GRAPH (L4/S3.DE=TEK613,PO=l,5,OV) TIME (LE=6 . 0 , UM= ' SECONDS ' ) , . . . 
P2(LE=4,NI=4,LO=-.5,UN='R.AD' , SC=0 , 50 ,PO=6 . 0 ) , . . . 
PM2(LE=4,NI=4,LO=-.5,UN='RAD' ,SC=0.50) , . , . 

RRD2 ( LE=4 , MI=4 , LO=- . 5 , SC=0 . 50 , AX=OMIT ) 
*RAPH(L5/S3,DE=TEK613,PO=l, .5) TIME (LE=6 . 0 , UN= ' SECONDS ' ) , . . . 

^ ER1(LE=3,MI=3,LO=0. ,UN='RAD' ,SC=0.005) , . . . 

ER2aH=3,MI=3,LO=0. ,UN='R.AD' , SC=0 . 005 , PO=6 , ) 

LABEL (LI) PHASE PLANE (VOLTAGE SOUR. DRV. WITH LOAD) 

LABEL (L3) STEP RESPONSE (VOLTAGE SOUR. DRV. WITH LOAD) 

LABEL (L5) error VS TIME (VOLTAGE SOUR. DRV. WITH LOAD) 

END 

STOP 
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APPENDIX D 



DSL PROGRAM FOR VOLTAGE SOURCE DRIVE (GRAVITATIONAL 

TORQUES INCLUDED) 



TITLE SIMULATION PROGRAM OF THE ADAPTIVE MODEL (VOLTAGE SOURCE DRIVE) 
TITLE GRAVITATIONAL TOROUES INCLUDED 

PARAM K1=0.60,K2=10000. ;KM1=0.17,KH2=4,00,VSAT1=300. , K=1 , 0 ,T=0 .00025 
PARAM G=386.4,M1=0.248,M2=0.082,KT1=14.4,KT2=14.4,D1=15. ,D2=10. 

PARAM J1=0.033,J2=0.033,KV1=0.1012,KV2=0.1012,R1=0.91,R2=O.S1,K3=0.6 
PARAM REFl =1 . 0 , REF2=1 . 0 ; L=0 . 0001 , BM1=0 . 0429 , 3M2=0 . 0429 , VSAT2=1 50 . 
IMTGER Ml ,M2,FLAG1 ,FLAG2 
INITIAL 

N1=0 

FLAG1=0 

N2=0 

FLAG2=0 

P1=0.0 

P2=0.0 

P1D0T=0, 

P2D0T=0. 

P1DDT=0 . 

P2DDT=0. 

PM1=0.0 

PM2=0.0 

PM1DT=0. 

PM2DT=0. 

PM1DDT=0. 

PM2DDT=0 . 

K1D0T=0. 

K2D0T=0 . 

TL1=0. 

TL2=0. 

TL11=0. 

TL22=0. 

PP1=L/R1 

P?2=L/R2 

TM1=0. 

TM2=0. 

IM1=0. 

IM2=0. 

A1=SQRT ( 2 . *KM1 *VSAT1 ) 

A2=SQRT ( 2 . *KM2*VSAT2 ) 

DERIVATIVE 

RR1=REF1*STEP(0.0) 

RR2=REF2*STEP(0.0) 

RRD2=TRANSP ( 50 , 0 , 0 , 0 . 1 0 , RR2 ) 

E1=RR1-P1 

E2=RRD2-P2 

NO SORT 

D11=(M1+M2)*(D1**2)+M2*(D2*’^2)+2’^M2*D1*D2*C0S(PM2) 

D12=M2*(D2’'*2)+M2*Dl*D2’'COS(PM2) 

D22=H2*(D2**2) 

D122=-M2*D1«D2*SIN(PM2) 

D211=D122 

D112=D211 

D121=D112 

D212=D121 

D221=D212 

PMT=PM1+PM2 

G1=(M1+M2)*G*D1*C0S(PM1)+M2*G*D2*C0S(PMT) 

G2=M2*G*D2*COS(PMT) 

TL1=D12*PM2DDT+D122*PM2DT^*2 + 2’^D112*PM1DT*PM2DT+G1 

TL2=D12*PM1DDT-D211*PM1DT**2+G2 

JT0T1=J1+D11 
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JTOT2=J2+D22 

IF(Ei .LT.O.O)X1DOT=-A1’^K1*SORT(ABS(E1)) 

I?(E1 .GE.O .0)X1DOT=A1-K1*SORT{E1) 
I?(E2.LT.O.O)X2DOT=-A2^K3^SORT{ABS(E2)) 

IF (E2 . GE . 0 . 0 )X2DOT=A2’^X3*SORT (E2 ) 

SORT 

X1D0TE=X1D0T-K?1D0T 

K?1D0T=P1D0T*K 

V1=LIHIT(-VSAT1,VSAT1,K2^X1D0TE) 

NO SORT 

IF(FLAG1.E0.1)GO TO 5 

IF( VI , LT . VSATl . AND . TIME . GT . 0 . 00005 ) FLAG1=1 
MSW1=N1 

5 CONTINUE 
SORT 

?1DDT=KM1’='V1 
? 1 DOT= INTGRL ( 0 . 0 , ? IDDT ) 

P1=IMTGRL(0.0,?1DOT) 

V?1=V1-{KV1’^PH1DT) 

IM1=REALPL(0.0,PP1,VP1/R1) 

TM1=KT1^IM1 

TNET1=TM1-PM1DT*BM1-TL1 
?M1DDT={ 1 . / JTOTl )^TMET1 
?H1BI=INTGRL(0.0,?H1DDT) 

phi=intgrl{o,o,f:iidt) 

X2DOTE=X2DOT-KP2DOT 

KP2DOT=P2DOT’'K 

V2=LIMIT ( -VSAT2 , VSAT2 , K2^X2DOTE ) 

NOSORT 

IF(FLAG2.E0.1)G0 to 5 

IF { V2 . LT . VSAT2 . AI'ID . TIME . GT . 0 . 0000 5 ) FLAG2=1 
NSW2=N2 

6 CONTINUE 
SORT 

P2DDT=KM2*V2 

P2DOT= INTGRL (0 . 0 , P2DDT) 

P2= INTGRL ( 0 , 0 , P 2DOT ) 

VP2=V2-(KV2’^?H2DT) 

IH2=REALPL { 0 . 0 , PP2 , VP2/R2 ) 

TM2=KT2*IH2 

TMET2=TN2 -PH2DT*3M2 - TL 2 
PM2DDT= ( 1 . / JTOT2 ) ^TNET2 
?H2DT=IMTGRL(0 .0 ,PH2DDT) 

PM2=INIGRL ( 0 . 0 , PM2DT ) 

SAMPLE 

NOSORT 

IF(N2.EQ.O)GO TO 21 
IF (Ml. EO. 0^0 TO 20 
P2=?M2 - 
P1=?M1 

KS2=A3S(2.*PM2/(V2*( (N2”T)**2))) 
KS1=ABS(2.=^PM1/(V1’^( (Nm)**2)) ) 

I F ( FLAG2 . EO . 0 ) KM2=KS2 
IF(FLAG1 .E6.0)KM1=KS1 
IF(N2.GE.2)?M2DTL=(PH2-PH22L)/(2.*T) 

I F (N1 . GE . 2 ) PH1DTL= ( PHI -PM12L ) / ( 2 . ’^T ) 

I F ( FLAG2 . EO . 0 ) P2DOT= ( 2 . >^ ( ( PM2 - PM2LST ) /T ) ) - PM2DTL 
I F ( FLAGl . EO . 0 ) P1D0T= ( 2 . * ( ( PHI -PHILST ) /T ) ) -PHIDTL 
IF(N2.EQ.NSW2.AND.FLAG2.E0.1)G0 TO 21 
IF(M1.E0.NSW1. AND. FLAGl. EQ.l)GO TO 20 
IF ■ FLAGS . EO . 1 ) P2DOT= ( 2 . * ( rPM2 -PM2LST ) /T ) ) -PM2DTL 
IF FLAGl . EQ . 1 )P1D0T= ( 2 . ’^ ( (PHI -PHILST )/T ) ) -PHIDTL 
21 M2=M2+1 

20 M1=N1+1 

PH2DTL=P2DOT 

PH1DTL=P1D0T 

P2DOT=PH2DTL 

?H22L=PM2LST 

PM12L=PM1LST 



PM2LST=PM2 

PM1LST=PM1 

* ER1=ABS(RR1-PM1) 

* ER2=ABS(RR2-PH2) 

SORT 

TERMINAL 

FINISH PM1=1.00 
METHOD RKSFX 

CONTRL FINTIM=0 ,70000 , DELT=0 .00005 , DELS= . 00025 

PRINT 0.010, KIDOT , PIDOT , PMIDT , X2DOT , P2DOT , PM2DT , PMl , RRl , PM2 , RR2 , TMl , TM2 
SAVE (SI) 0.01, XIDOT, PIDOT, PMIDT, X2DOT,P2DOT,PM2DT, PMl, PM2 
SAVE (S3) 0.01, P1,PM1,REF1,P2,PM2,REF2,RR1,RRD2 

(L1/S1,DE=TEK618,P0=1, .5) PMl (LE=6 . 0 , UN= ' RAD ' , L0=-0 . 10 , SC=0 . 10 , . . . 
NI=12) , PIDOT (LE=4, MI=4,LO=-2.0,UN=' RAD/SEC ' , SC=2 . 0 , PO=6 . 0 ) , . . . 
X1DOT(LE=4,NI=4,LO=-2.0,SC=2.0,UN='RAD/SEC' ,.AX=OMIT) , . . . 
PM1DT(LE=4 , NI=4 , LO=-2 . 0 , UN= ' RAD/SEC ' , SC=2 . 0 ) 

(L2/S1 , DE=TEK618 ,0V , PO=l , 5 ) PM2 ( LE=6 . 0 , UN= ' RAD ' , L0=-0 .10 

SC=0. 10,NI = 12) , P2DOT(LE=4,NI=4,LO=0.0,UN=' RAD/SEC ' ,SC=5.0,'. . . 
PO=6.) ,X2DOT(LE=4,MI=4,LO=0.0,UN=' RAD/SEC ' , SC=5 . 0 , AX=0MIT) , . . . 
PH2DT ( LE=4 , NI=4 , L0=0 . 0 , UN= ' RAD/SEC ' , SC=5 . 0 ) 

(L3/S3,DE=TEK618,PO=l, .5) TIME (LE=6 . 0 , UN= ' SECONDS ') , . . . 

P 1 ( LE=4 , NI=4 , LO=- . 5 , UN= ' RAD ' , SC=0 . 50 , PO=6 . 0 ) 

PM1(LE=4,NI=4,L0=- .5,UN=' R.AD' ,SC=0.50), . . . 

RRl ( LE=4 , MI=4 , LO=- . 5 , SC=0 . 50 , AX=OMIT ) 

(L4/S3.DE=TEr;olS,PO=l ,5,0V) TIME (LE=6 .0 ,UN=' SECONDS ' ) , . . . 
P2(^LE=4,NI=4,LO=-.5,UN='R.^D' , SC=0 . 50 , PO=6 . 0) , . . . 
PM2(LE=4,MI=4,LO=-.5,UN='RAD' ,SC=0.50), . . . 

RRD2 ( LE=4 , NI=4 , LO=- . 5 , SC=0 . 50 , AX=OMIT ) 
’^RAPH(L5/S3,DE=TEK613,PO=l , .5) TIME (LE=6 . 0 , UN= ' SECONDS ' ) , . . . 

^ ER1(LE=8,NI=8,LO=0 . ,UN='RAD' ,SC=0.005) , . . , 

ER2(LE=3,NI=8,LO=0 . ,UN='RAD' , SC=0 . 005 , PO=6 . ) 

LABEL (LI) PHASE PLANE (VOLTAGE SOUR. DRV. WITH LOAD) 

LABEL (L3) STEP RESPONSE (VOLTAGE SOUR. DRV. WITH LOAD) 

LABEL (L5) error VS TIME (VOLTAGE SOUR. DRV. WITH LOAD) 

END 

STOP 



GRAPH 

GRAPH 

GRAPH 

GRAPH 



176 



APPENDIX E 



DSL PROGRAM FOR CURRENT SOURCE DRIVE (NO 
GRAVITATIONAL TORQUES) 



TITLE SIMULATION PROGRAM OF THE ADAPTIVE MODEL (CURRENT SOURCE DRIVE) 
TITLE NO GRAVITATIONAL TORQUES 

PARAM K1=0.60,K2=10000. , KM1=0 . 17 ,KM2=4 . 00 , VSAT=100 . , K=1 . 0 ,T=0 . 00025 
PARAM G=386.4,M1=0.243,M2=0 .082,KT1=14.4,KT2=14.4,D1=15. ,D2=10. 

PARAM J1=0 .033 , J2=0 . 033 ,KV1=0 . 1012 , KV2=0 . 1012 , R1=0 . 91 , R2=0 . 91 ,K3=0 . 6 

PARAM REF1=1 . 0 , REF2=1 . 0 , L=0 . 0001 , BM1=0 . 0429 , BM2=0 . 0429 

PARAM RS=0. 1,VSATA1=100,KA1=150,VSATA2=50,KA2=75 

IMTGER N1,N2,FLAG1,FLAG2 

INITIAL 

N1=0 

FLAG 1=0 

N2=0 

FLAG2=0 

P1=0.0 

P2=0.0 

P1DOT=0. 

P2DOT=0 , 

P1DDT=0. 

P2DDT=0. 

?M1=0.0 
PM2=0 . 0 
PM1DT=0. 

PM2DT=0 . 

PM1DDT=0. 

PM2DDT=0 . 

X1DOT=0 . 

X2DOT=0 . 

TL1=0. 

TL2=0. 

TL11=0. 

TL22=0. 

KF=1 ./RS 

RTOTl=Rl+RS 

RTOT2=R2+RS 

PPl=L/RTOTl 

PP2=L/RT0T2 

TM1=0. 

TM2=0. 

IM1=0 . 

IM2=0 . 

A1=S0RT ( 2 . *KM1*VSAT ) 

A2=SQRT ( 2 . ’^KM2^VSAT ) 

DERIVATIVE 

RR1=REF1’^SIN(3.14*TIME) 

RR2=REF2^SIN( 3 . 14^TIME ) 

RRD1=TRANSP(50,0.0,0.15,RR1) 

E1=RR1-P1 

E2=RR2-P2 

NO SORT 

Dll=(Ml+M2)’^(Dl*^2)+M2*(D2^^2)+2’^M2*Dl*D2*COS(PM2) 

D12=M2*(D2’'’^2)+M2^Dl*D2^COS(PM2) 

D22=M2*(D2*=^2) 

D122=-M2*D1*D2*SIN(PM2) 

D211=D122 

D112=D211 

D121=D112 

D212=D121 

D221=D212 

PMT=PM1+PM2 

G1=(M1+M2)*G’^D1*C0S(PM1)+M2*G>^D2^C0S(PMT) 
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* G2=M2*G*D2*COS(PMT) 

TL1=D12*PM2DDT+D122*PM2DT*’^2+2*D112*PM1DT’^PM2DT 

TL2=D12*PM1DDT-D211*PM1DT*’^2 

JT0T1=J1+D11 

JTOT2=J2+D22 

IF(E1.LT.O.O)X1DOT=-A1*K1*SQRT(ABS(E1) ) 

I F ( E 1 . GE . 0 . 0 ) XI D0T=A1 *K1 ^SORT (El) 

IF (E2 . LT . 0 . 0)X2DOT=-A2*K3*SQRT ( ABS (E2 ) ) 
IF(E2.GE.O.O)X2DOT=A2^K3*SQRT(E2) 

SORT 

X1D0TE=X1D0T-KP1D0T 

KP1D0T=P1D0T*K 

V1=LIMIT ( -VSAT , VSAT , K2*X1D0TE) 

NOSORT 

IF(FLAG1 .E0.1)G0 TO 5 

IF(V1 .LT. VSAT. AND. TIME. GT.O, 00005 )FLAG1=1 
NSW1=M1 

5 CONTINUE 
SORT 

P1DDT=KM1*V1 

P1DOT=IMTGRL(0.0,P1DDT) 

P1=IMTGRL(0.0,P1DOT) 

VF1=KF*IM1*RS 

VA1=V1-VF1 

VM1=LIHIT( -VSATAl ,VSATA1 ,VA1*KA1) 

VD1=VM1-KV1*PM1DT 

IM1=REALPL(0.0,?P1,VD1/RTOT1) 

TM1=KT1=*'IM1 

TNET1=TM1-PM1DT*BM1-TL1 
PM1DDT=( 1 . / JTOTl ) *TNET1 
PH1DT=IMTGRL ( 0 . 0 , PMIDDT ) 

PM1=INTGRL(0.0,PM1DT) 

X2DOTE=X2DOT-KP2DOT 

KP2DOT=P2DOT*X 

V2=LIMIT ( - VSAT , VSAT , K2*X2DOTE ) 

NOSORT 

IF(FL.AG2.EQ.l)GO TO 6 

I F ( V2 . LT . VSAT . AND . TIME . GT . 0 . 0000 5 ) FLAG2=1 
MSW2=N2 

6 CONTINUE 
SORT 

P2DDT=KM2’^V2 
P2DOT=IMTGRL (0.0, P2DDT ) 

P2=INTGRL(0.0,P2DOT) 

VF2=KF*IM2*RS 

VA2=V2-VF2 

VM2=LIMIT ( -VSATA2 , VSATA2 , VA2*KA2 ) 

VD2=VM2-KV2*?H2DT 

IH2=REALPL ( 0 . 0 , PP2 , VD2/RTOT2 ) 

TM2=KT2*IH2 

TNET2=TM2-PM2DT*BM2-TL2 
PM2DDT=(1 ./JTOT2)*TNET2 
PM2DT=INTGRL ( 0 . 0 , PM2DDT ) 

PH2=INTGRL (0 . 0 , PM2DT ) 

SAMPLE 

NOSORT 

IF(M2.EO.O)GO TO 21 
IF(N1 .EQ.O)GO TO 20 
P2=PM2 
P1=PM1 

KS2=ABS(2.*PM2/(V2*((N2*T)**2))) 
KS1=ABS(2.*PM1/(V1*( (N1*T)**2)) ) 
IF(FLAG2.EQ.0)KH2=KS2 
IF(FLAG1 .EQ.0)KH1=KS1 
IF(N2.GE.2)PM2DTL=(PM2-PM22L)/(2.*T) 

IF (Ml . GE . 2 ) PM1DTL= ( PMl -PMl 2L ) / ( 2 . *T ) 

IF ( FLAG2 . EQ . 0 ) P2DOT= ( 2 . * ( ( PH2-PM2LST ) /T ) ) -PM2DTL 
IF(FLAG1 .EQ.O)P1DOT=(2.’^( (PH1-PM1LST)/T) )-PMlDTL 
IF(N2.EQ.NSW2..AMD.FLAG2.EQ.l)GO TO 21 
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IF(N1.E0.NSW1.AND.FLAG1.EQ.1)G0 TO 20 
IF ( FLAGS . EQ . 1 ) P2D0T= ( 2 . ( (PM2-PM2LST ) /T ) ) -PM2DTL 
IF ( FLAGl . EQ . 1 ) P1D0T= ( 2 . * ( ( PMl -PMILST ) /T ) ) - PMIDTL 
21 N2=M2+1 

20 N1=M1+1 

PM2DTL=P2DOT 

PM1DTL=P1D0T 

PM22L=PM2LST 

PM12L=PH1LST 

PM2LST=PM2 

PM1LST=PM1 

ER1=ABS(RR1-PM1) 

ER2=ABS(RR2-PM2) 

SORT 

TERMINAL 
FINISH PM1=1,0 
METHOD RKSFK 

CONTRL FINTIM=1 . 00000 , DELT=0 . 00005 , DELS=0 . 00025 

PRINT 0.020, XIDOT , PMIDT , PIDOT , X2DOT , P2DOT , PM2DT , PMl , RRl , TLl , PM2 , RR2 , TL2 
SAVE (SI) 0.001, XIDOT, PIDOT, PMIDT, X2DOT,P2DOT,PM2DT, PMl, PM2 
SAVE (S3) 0.001, P1,PM1,REF1,P2,PM2,REF2,ER1,ER2,RR1,RR2 
GRAPH (L1/S1,DE=TEK613,P0=1, .5) PMl (LE=6 . 0 , UN= ' RAD ' , LO=-0 . 10 , SC=0 . 1 , . . . 

NI=12) ,PlDOT(LE=4,NI=6,LO=-6. , UN= ' RAD/SEC ' , SC=3 . 0 , PO=6 . 0 ) , . , . 

XlDOT(LE=4,MI=6,SC=3.0,LO=-6. , UN= ' RAD/SEC ' ,AX=0MIT) , . . . 

PMIDT (LE=4,NI=6,L0=-6. , UN= ' RAD/SEC ' ,SC=3.0) 

GRAPH (L2/S1,DE=TEK613,P0=1,5,0V) PM2 (LE=6 . 0 , UN= ' RAD ' ,LO=-O.l 

SC=0,10,NI=12) , P2D0T(LE=4, MI=4,LO=-6.0,UN=' RAD/SEC ' ,SC=3.0 

P0=6.0) , X2DOT(LE=4,NI=4,LO=-6.0,UM='RAD/SEC' , SC=3 . 0 , AX=OMIT) , . . . 
PM2DT (LE=4 , NI=4 , LO=-6 . 0 , UN= ' RAD/SEC ' , SC=3 . 0 ) 

GRAPH (L3/S3,DE=TEK513,PO=l, .5) TIME (LE=6 , 0 ,UN= ' SECONDS ' ) , . . . 

P1(LE=4,NI=3,LO=0.0,UN='RAD' , SC=0 . 50 , P0=6 . 0) , . . . 

PM1(LE=4,NI=3,L0=0.0,UN='RAD' ,SC=0.50) , . . . 

RRl (LE=4 ,NI=3 , L0=0 . 0 , SC=0 . 50 , AX=OMIT) 

GRAPH (L4/S3 ,DE=TEK613,PO=l,5,OV) TIME (LE=6 . 0 , UN= ' SECONDS ' ) , . . . 

P2 (LE=4 , MI=3 , L0=0 . 0 , UN= ' RAD ' , SC=0 . 5 , PO=6 . 0 ) , . . . 

PM2(LE=4,NI=3,LO=0.0,UN='RAD' ,SC=0,5) , , . . 

RR2 ( LE=4 ,NI=3 , LO=0 . 0 , SC=0 . 5 , AX=OMIT ) 

GRAPH (L5/S3,DE=TEK613,PO=l, .5) TIME (LE=6 . 0 , UN= ' SECONDS '),,. . 

ER1(LE=3,NI=3,L0=0. ,UN='RAD' ,SC=0.05) , . . . 

ER2(LE=3,NI=3,L0=0. ,UN='RAD' , SC=0 .05 , P0=6 . ) 

LABEL (LI) PHASE PLANE (CURRENT SOUR. DRV. WITH LOAD) 

LABEL (L3) SINE RESPONSE (CURRENT SOUR. DRV. WITH LOAD) 

LABEL (L5) ERROR VS TIME (CURRENT SOUR. DRV. WITH LOAD) 

END 

STOP 
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APPENDIX F 



DSL PROGRAM FOR CURRENT SOURCE DRIVE (GRAVITATIONAL 

TORQUES INCLUDED) 



TITLE SIMULATION PROGRAM OF THE ADAPTIVE MODEL (CURRENT SOURCE DRIVE) 
TITLE GRAVITATIONAL TORQUES INCLUDED 

PARAM K1=0 . 60 , K2=10000 . , KM1=0 . 11 , KM2=4 . 00 , VSAT=300 . , K=1 . 0 , T=0 . 00025 
PARAM G=386.4,M1=0 .248,M2=0.082,KT1=14.4,KT2=14.4,D1=15. ,D2=10. 

PARAM Jl=0.033, J2=0.033,KV1=0.1012,KV2=0.1012,R1=0.91,R2=0.91,K3=0.6 
• PARAM REF1=1 . 0 , REF2=1 . 0 , L=0 . 0001 , BM1=0 . 0429 , BM2=0 . 0429 
PARAM RS=0 . 1 , VSATAl =300 , KA1 = 350 , VSATA2=1 50 , KA2=17 5 
IMTGER N1,N2,FLAG1,FLAG2 
INITIAL 

N1=0 

FLAG1=0 

N2=0 

FLAG 2=0 

P1=0.0 

P2=0.0 

P1D0T=0. 

P2D0T=0. 

P1DDT=0. 

P2DDT=0. 

PM1=0.0 
PM2=0 . 0 
PM1DT=0. 

PH2DT=0 . 

PM1DDT=0 . 

PM2DDT=0 . 

X1D0T=0. 

X2DOT=0 . 

TL1=0. 

TL2=0 . 

TL11=0. 

TL22=0. 

KF=1 ./RS 

RTOTl=Rl+RS 

RTOT2=R2+RS 

PPl=L/RTOTl 

PP2=L/RTOT2 

TM1=0. 

TM2=0. 

IM1=0. 

IM2=0 . 

A1=SQRT ( 2 . *KM1 *VSAT ) 

A2=SQRT ( 2 . *KM2*VSAT) 

DERIVATIVE 

RR1=REF1*SIN(3 . 14*TIME ) 

RR2=REF2*SIN( 3 . 14*TIHE ) 

* RRD1=TRANSP(50,0.0,0.15,RR1) 

E1=RR1-P1 

E2=RR2-P2 

NOSORT 

D11 = (M1+M2)=*'(D1**2)+M2’^(D2**2)+2*M2’^D1*D2*C0S(PM2) 
D12=M2’^(D2^*2)+M2*Dl’^D2^COS(PM2) 

D22=M2’^(D2**2) 

D122=-M2*D1*D2’^SIN(PM2) 

D211=D122 

D112=D211 

D121=D112 

D212=D121 

D221=D212 

PMT=PM1+PM2 

G1=(M1+M2)*G*D1*C0S(PM1)+M2*G*D2*C0S(PMT) 



ISO 



G2=H2*G*D2*C0S(PMT) 

TL1=D12’^PH2DDT+D122*PH2DT*’^2+2’^D112*PM1DT*PM2DT+G1 

TL2=D12^PM1DDT-D211*PM1DT’^*2+G2 

JT0T1=J1+D11 

JTOT2=J2+D22 

I?(E1 .LT.O.O)X1DOT=-A1*K1’^SORT(ABS(E1)) 

IF(E1 .GE.O.O)X1DOT=A1^K1^SORT(E1) 
IF{E2.LT.O.O)X2DOT=-A2^K3*SORT(ABS(E2) ) 
IF(E2.GE.O.O)X2DOT=A2’^K3’^SQRT(E2) 

SORT 

X1D0TE=X1D0T-KP1D0T 

KP1D0T=P1D0T*K 

V1=LIMIT ( -VSAT , VSAT , K2*X1D0TE ) 

NO SORT 

IF(FLAG1.E0.1)G0 TO 5 

IF (VI .LT. VSAT. AND. TIME. GT.O. 00005 )FLAG1=1 
NSW1=M1 

5 CONTINUE 
SORT 

P1DDT=KH1*V1 

P1DOT=INTGRL(0.0,P1DDT) 

P1=INTGRL(0.0,P1DOT) 

VF1=KF^IM1’^RS 

VA1=V1-VF1 

VM1=LIMIT( -VS.ATA1 , VSATAl ,VA1*KA1 ) 

VD1=VM1-KV1^PH1DT 

IM1=REALPL(0.0,PP1,VD1/RTOT1) 

TM1=KT1*IM1 

TMET1=TM1-PM1DT*BM1-TL1 
PH1DDT=(1 ./JT0T1)^TNET1 
PH1DT=IMTGRL(0 . 0 , PMIDDT) 

PM1=INTGRL(0.0,PM1DT) 

X2DOTE=X2DOT-KP2DOT 

KP2DOT=P2DOT’^K 

V2=LIMIT( -VSAT , VSAT ,K2’^X2DOTE) 

NO SORT 

IF(FLAG2.E0.1)G0 TO 6 

IF ( V2 . LT . VSAT . AND . TIME . GT . 0 . 00005 ) FLAG2=1 
NSW2=N2 

6 CONTINUE 
SORT 

P2DDT=KM2^V2 
P2DOT=INTGRL (0.0, P2DDT ) 

P2=IMTGRL(0.0,P2DOT) 

VF2=KF*IM2*RS 

VA2=V2-VF2 

VM2=LIHIT ( - VSATA2 , VSATA2 , VA2*KA2 ) 

VD2=VM2-KV2’^PM2DT 

IM2=REALPL(0 . 0 ,PP2 , VD2/RTOT2) 

TM2=KT2*IM2 

TMET2=TM2 -PM2DT*BM2 -TL2 
PM2DDT= ( 1 . / JTOT2 ) *TMET2 
PM2DT= INTGRL (0.0, PM2DDT ) 

PM2=INTGRL ( 0 . 0 , PH2DT) 

SAMPLE 

NOSORT 

IF(M2.EQ.0)G0 TO 21 
IF(n1 .EO,0)GO TO 20 
P2=PM2 ' 

P1=PM1 

KS2=ABS(2.*PM2/(V2*((N2*T)**2))) 

KS1=ABS(2.*PM1/(V1*( (N1*T)**2)) ) 

IF(FLAG2.EQ.0)KM2=KS2 

IF(FLAG1 .EQ.0)KM1=KS1 

IF (N2 . GE . 2 ) PH2DTL= ( PH2-PM22L ) / ( 2 . *T ) 

I F ( Ml . GE . 2 ) PM1DTL= ( PMl -PMl 2L ) / ( 2 . *T ) 

I F ( FLAG2 . EO . 0 ) P2DOT= ( 2 . * ( ( PH2-PH2LST ) /T ) ) -PM2DTL 
IF ( FLAGl . EQ . 0 ) P1D0T= ( 2 , * ( ( PMl -PMILST ) /T ) ) -PMIDTL 
IF(M2,EQ.NSW2.AND.FLAG2.EQ.1)G0 TO 21 



ISl 



IF(N1.E0.MSW1.AND.FLAG1.EQ.1)G0 TO 20 
IF(FLAG2.EQ. l)P2DOT=(2.^((PM2-PM2L5T)/T))-PM2DTL 
IF ( FLAGl . EQ . 1 ) P1D0T= ( 2 . * ( (PHI -PMILST ) /T ) ) -PMIDTL 
21 N2=N2+1 

20 N1=M1+1 . 

PM2DTL=P2DOT 

PH1DTL=P1D0T 

PM22L=PH2LST 

PM12L=PM1LST 

PM2LST=PM2 

PH1L5T=PH1 

ER1=ABS(RR1-PM1) 

ER2=ABS(RR2-PM2) 

SORT 

TERMINAL 
FINISH PM1=1.0 
METHOD RKSFX 

CONTRL FIMTIM=1 .00000 , DELT=0 . 00005 , DELS=0 . 00025 

PRINT 0.020, XIDOT , PMIDT , PIDOT , X2DOT , P2DOT , PM2DT , PHI , RRl , TLl , PH2., RR2 , TL2 
SAVE (SI) 0.001, XIDOT, PIDOT, PMIDT, X2DOT,P2DOT,PM2DT, PHI, PM2 
SAVE (S3) 0.001, P1,PM1,REF1,P2,PM2,REF2,ER1,ER2,RR1,RR2 
GRAPH (L1/S1 ,DE=TEKd 18,PO=1, .5) PMl (LE=6 . 0 , UM= ' RAD ' ,LO=-0 . 1 , SC=0 . 1 , . . . 

MI=12) ,PlDOT(LE=4,NI=6,LO=-6. , UN= ' RAD/SEC ' , SC=3 .0 ,PO=6 .0) , . . . 
XlDOT(LE=4,MI=6,SC=3.0,LO=-6. , UN= ' R.AD/SEC ' ,.AX=OMIT) , . . . 

PMIDT ( LE=4 , NI=6 , LO=-6 . , UN= ' R.AD/SEC ' , SC=3 . 0 ) 

GRAPH (L2/Sl,DE=TEK613,PO=l,5,OV) PM2 (LE=6 . 0 , UN= ' RAD ' ,LO=-O.l, . . . 

SC=0.10,NI=12) , P2DOT(LE=4,MI=4,LO=-6.0,UM='RAD/SEC ,SC=3.0, . . . 
P0=6.0) , X2DOT(LE=4,NI=4,LO=-6.0,UN=' RAD/SEC ' ,SC=3. ,AX=OHIT) , . . . 
PM2DT(LE=4,NI=4,LO=-6.0,UN=' RAD/SEC ' ,SC=3.0) 

GRAPH (L3/S3,DE=TEXSia,PO=l, .5) TIME (LE=6 . 0 , UM= ‘ SECONDS ' ) , . . . 
P1(LE=4,NI=3,LO=0.0,UN='RAD' , SC=0 . 50 , P0=6 . 0 ) , . . . 
PM1(LE=4,NI=3,LO=0.0,UN='RAD' ,SC=0.50) , . . . 

RRl ( LE=4 , MI=3 , LO=0 . 0 , SC=0 . 50 , AX=0HIT) 

GRAPH (L4/S3.DE=TEK618,PO=l,5,OV) TIME (LE=6 . 0 , UM= ' SECONDS ' ) , . . . 

P2 (LE=4 , NI=3 , L0=0 . 0 , UM= ' RAD ' , SC=0 . 5 , PO=6 . 0 ) , . . . 
PM2(LE=4,NI=3,LO=0.0,UN='RAD' ,SC=0.5) , . . . 

RR2 (LE=4 , MI=3 , LO=0 . 0 , SC=0 . 5 , AX=0MIT) 

GRAPH (L5/S3,DE=TEK618,PO=l, .5) TIME (LE=6 . 0 , UH= ' SECONDS '),... 
ER1(LE=8,NI = 8,LO=0. ,UM='RAD' ,SC=0.05) , . . . 

ER2(LE=8,NI=8,LO=0. ,UN='R.AD' , SC=0 . 05 , P0=6 . ) 

LABEL (LI) PHASE PLANE (CURRENT SOUR. DRV. WITH LOAD) 

LABEL (L3) SINE RESPONSE (CURRENT SOUR. DRV. WITH LOAD) 

LABEL (L5) error VS TIME (CURRENT SOUR. DRV. WITH LOAD) 

END 

STOP 
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